Commit 39e5308b authored by Yangbo Lu's avatar Yangbo Lu Committed by David S. Miller
Browse files

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: default avatarYangbo Lu <yangbo.lu@nxp.com>
Acked-by: default avatarRichard Cochran <richardcochran@gmail.com>
Signed-off-by: default avatarDavid S. Miller <davem@davemloft.net>
parent 682eaad9

drivers/net/ethernet/mscc/ocelot.c

+53 −0
Original line number Diff line number Diff line
@@ -6,6 +6,7 @@ 
 */

#include <linux/dsa/ocelot.h>

#include <linux/if_bridge.h>

#include <linux/ptp_classify.h>

#include <soc/mscc/ocelot_vcap.h>

#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;

drivers/net/ethernet/mscc/ocelot_net.c

+4 −4
Original line number Diff line number Diff line
@@ -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);

include/soc/mscc/ocelot.h

+7 −1
Original line number Diff line number Diff line
@@ -691,6 +691,7 @@ struct ocelot_policer { 


struct ocelot_skb_cb {

	struct sk_buff *clone;

	u8 ptp_cmd;

	u8 ts_id;

};
@@ -748,15 +749,16 @@ u32 __ocelot_target_read_ix(struct ocelot *ocelot, enum ocelot_target target, 
void __ocelot_target_write_ix(struct ocelot *ocelot, enum ocelot_target target,

			      u32 val, u32 reg, u32 offset);



/* Packet I/O */

#if IS_ENABLED(CONFIG_MSCC_OCELOT_SWITCH_LIB)



/* Packet I/O */

bool ocelot_can_inject(struct ocelot *ocelot, int grp);

void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,

			      u32 rew_op, struct sk_buff *skb);

int ocelot_xtr_poll_frame(struct ocelot *ocelot, int grp, struct sk_buff **skb);

void ocelot_drain_cpu_queue(struct ocelot *ocelot, int grp);



u32 ocelot_ptp_rew_op(struct sk_buff *skb);

#else



static inline bool ocelot_can_inject(struct ocelot *ocelot, int grp)
@@ -780,6 +782,10 @@ static inline void ocelot_drain_cpu_queue(struct ocelot *ocelot, int grp) 
{

}



static inline u32 ocelot_ptp_rew_op(struct sk_buff *skb)

{

	return 0;

}

#endif



/* Hardware initialization */

net/dsa/Kconfig

+2 −0
Original line number Diff line number Diff line
@@ -111,6 +111,8 @@ config NET_DSA_TAG_RTL4_A 


config NET_DSA_TAG_OCELOT

	tristate "Tag driver for Ocelot family of switches, using NPI port"

	depends on MSCC_OCELOT_SWITCH_LIB || \

		   (MSCC_OCELOT_SWITCH_LIB=n && COMPILE_TEST)

	select PACKING

	help

	  Say Y or M if you want to enable NPI tagging for the Ocelot switches

net/dsa/tag_ocelot.c

+4 −23
Original line number Diff line number Diff line
@@ -5,33 +5,14 @@ 
#include <soc/mscc/ocelot.h>

#include "dsa_priv.h"



static void ocelot_xmit_ptp(struct dsa_port *dp, void *injection,

			    struct sk_buff *clone)

{

	struct ocelot *ocelot = dp->ds->priv;

	struct ocelot_port *ocelot_port;

	u64 rew_op;



	ocelot_port = ocelot->ports[dp->index];

	rew_op = ocelot_port->ptp_cmd;



	/* Retrieve timestamp ID populated inside OCELOT_SKB_CB(clone)->ts_id

	 * by ocelot_port_add_txtstamp_skb

	 */

	if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)

		rew_op |= OCELOT_SKB_CB(clone)->ts_id << 3;



	ocelot_ifh_set_rew_op(injection, rew_op);

}



static void ocelot_xmit_common(struct sk_buff *skb, struct net_device *netdev,

			       __be32 ifh_prefix, void **ifh)

{

	struct dsa_port *dp = dsa_slave_to_port(netdev);

	struct sk_buff *clone = OCELOT_SKB_CB(skb)->clone;

	struct dsa_switch *ds = dp->ds;

	void *injection;

	__be32 *prefix;

	u32 rew_op = 0;



	injection = skb_push(skb, OCELOT_TAG_LEN);

	prefix = skb_push(skb, OCELOT_SHORT_PREFIX_LEN);
@@ -42,9 +23,9 @@ static void ocelot_xmit_common(struct sk_buff *skb, struct net_device *netdev, 
	ocelot_ifh_set_src(injection, ds->num_ports);

	ocelot_ifh_set_qos_class(injection, skb->priority);



	/* TX timestamping was requested */

	if (clone)

		ocelot_xmit_ptp(dp, injection, clone);

	rew_op = ocelot_ptp_rew_op(skb);

	if (rew_op)

		ocelot_ifh_set_rew_op(injection, rew_op);



	*ifh = injection;

}