net: dsa: microchip: ptp: add packet transmission timestamping (ab32f56a) · Commits · EulixOS / Software / Kernel

drivers/net/dsa/microchip/ksz_common.c

+14 −0

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

	#include <linux/delay.h>		#include <linux/delay.h>
			#include <linux/dsa/ksz_common.h>
	#include <linux/export.h>		#include <linux/export.h>
	#include <linux/gpio/consumer.h>		#include <linux/gpio/consumer.h>
	#include <linux/kernel.h>		#include <linux/kernel.h>
	@@ -2539,6 +2540,17 @@ static enum dsa_tag_protocol ksz_get_tag_protocol(struct dsa_switch *ds,
	return proto;		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->xmit_work_fn = ksz_port_deferred_xmit;

			return 0;
			}

	static int ksz_port_vlan_filtering(struct dsa_switch *ds, int port,		static int ksz_port_vlan_filtering(struct dsa_switch *ds, int port,
	bool flag, struct netlink_ext_ack *extack)		bool flag, struct netlink_ext_ack *extack)
	{		{
	@@ -2954,6 +2966,7 @@ static int ksz_switch_detect(struct ksz_device *dev)

	static const struct dsa_switch_ops ksz_switch_ops = {		static const struct dsa_switch_ops ksz_switch_ops = {
	.get_tag_protocol = ksz_get_tag_protocol,		.get_tag_protocol = ksz_get_tag_protocol,
			.connect_tag_protocol = ksz_connect_tag_protocol,
	.get_phy_flags = ksz_get_phy_flags,		.get_phy_flags = ksz_get_phy_flags,
	.setup = ksz_setup,		.setup = ksz_setup,
	.teardown = ksz_teardown,		.teardown = ksz_teardown,
	@@ -2991,6 +3004,7 @@ static const struct dsa_switch_ops ksz_switch_ops = {
	.get_ts_info = ksz_get_ts_info,		.get_ts_info = ksz_get_ts_info,
	.port_hwtstamp_get = ksz_hwtstamp_get,		.port_hwtstamp_get = ksz_hwtstamp_get,
	.port_hwtstamp_set = ksz_hwtstamp_set,		.port_hwtstamp_set = ksz_hwtstamp_set,
			.port_txtstamp = ksz_port_txtstamp,
	.port_rxtstamp = ksz_port_rxtstamp,		.port_rxtstamp = ksz_port_rxtstamp,
	};		};

drivers/net/dsa/microchip/ksz_common.h

+3 −0

Original line number	Original line	Diff line number	Diff line
	@@ -87,6 +87,7 @@ struct ksz_irq {
	struct ksz_ptp_irq {		struct ksz_ptp_irq {
	struct ksz_port *port;		struct ksz_port *port;
	u16 ts_reg;		u16 ts_reg;
			bool ts_en;
	char name[16];		char name[16];
	int num;		int num;
	};		};
	@@ -116,6 +117,8 @@ struct ksz_port {
	bool hwts_rx_en;		bool hwts_rx_en;
	struct ksz_irq ptpirq;		struct ksz_irq ptpirq;
	struct ksz_ptp_irq ptpmsg_irq[3];		struct ksz_ptp_irq ptpmsg_irq[3];
			ktime_t tstamp_msg;
			struct completion tstamp_msg_comp;
	#endif		#endif
	};		};

drivers/net/dsa/microchip/ksz_ptp.c

+114 −1

Original line number	Original line	Diff line number	Diff line
	@@ -18,6 +18,8 @@

	#define ptp_caps_to_data(d) container_of((d), struct ksz_ptp_data, caps)		#define ptp_caps_to_data(d) container_of((d), struct ksz_ptp_data, caps)
	#define ptp_data_to_ksz_dev(d) container_of((d), struct ksz_device, ptp_data)		#define ptp_data_to_ksz_dev(d) container_of((d), struct ksz_device, ptp_data)
			#define work_to_xmit_work(w) \
			container_of((w), struct ksz_deferred_xmit_work, work)

	/* Sub-nanoseconds-adj,max * sub-nanoseconds / 40ns * 1ns		/* Sub-nanoseconds-adj,max * sub-nanoseconds / 40ns * 1ns
	* = (2^30-1) * (2 ^ 32) / 40 ns * 1 ns = 6249999		* = (2^30-1) * (2 ^ 32) / 40 ns * 1 ns = 6249999
	@@ -111,9 +113,15 @@ static int ksz_set_hwtstamp_config(struct ksz_device *dev,

	switch (config->tx_type) {		switch (config->tx_type) {
	case HWTSTAMP_TX_OFF:		case HWTSTAMP_TX_OFF:
			prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = false;
			prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = false;
			prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = false;
	prt->hwts_tx_en = false;		prt->hwts_tx_en = false;
	break;		break;
	case HWTSTAMP_TX_ONESTEP_P2P:		case HWTSTAMP_TX_ONESTEP_P2P:
			prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = false;
			prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = true;
			prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = false;
	prt->hwts_tx_en = true;		prt->hwts_tx_en = true;
	break;		break;
	default:		default:
	@@ -232,6 +240,87 @@ bool ksz_port_rxtstamp(struct dsa_switch ds, int port, struct sk_buff skb,
	return false;		return false;
	}		}

			void ksz_port_txtstamp(struct dsa_switch ds, int port, struct sk_buff skb)
			{
			struct ksz_device *dev = ds->priv;
			struct ptp_header *hdr;
			struct sk_buff *clone;
			struct ksz_port *prt;
			unsigned int type;
			u8 ptp_msg_type;

			prt = &dev->ports[port];

			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_PDELAY_REQ:
			break;

			default:
			return;
			}

			clone = skb_clone_sk(skb);
			if (!clone)
			return;

			/* caching the value to be used in tag_ksz.c */
			KSZ_SKB_CB(skb)->clone = clone;
			}

			static void ksz_ptp_txtstamp_skb(struct ksz_device *dev,
			struct ksz_port prt, struct sk_buff skb)
			{
			struct skb_shared_hwtstamps hwtstamps = {};
			int ret;

			/* timeout must include DSA master to transmit data, tstamp latency,
			* IRQ latency and time for reading the time stamp.
			*/
			ret = wait_for_completion_timeout(&prt->tstamp_msg_comp,
			msecs_to_jiffies(100));
			if (!ret)
			return;

			hwtstamps.hwtstamp = prt->tstamp_msg;
			skb_complete_tx_timestamp(skb, &hwtstamps);
			}

			void ksz_port_deferred_xmit(struct kthread_work *work)
			{
			struct ksz_deferred_xmit_work *xmit_work = work_to_xmit_work(work);
			struct sk_buff clone, skb = xmit_work->skb;
			struct dsa_switch *ds = xmit_work->dp->ds;
			struct ksz_device *dev = ds->priv;
			struct ksz_port *prt;

			prt = &dev->ports[xmit_work->dp->index];

			clone = KSZ_SKB_CB(skb)->clone;

			skb_shinfo(clone)->tx_flags \|= SKBTX_IN_PROGRESS;

			reinit_completion(&prt->tstamp_msg_comp);

			dsa_enqueue_skb(skb, skb->dev);

			ksz_ptp_txtstamp_skb(dev, prt, clone);

			kfree(xmit_work);
			}

	static int _ksz_ptp_gettime(struct ksz_device dev, struct timespec64 ts)		static int _ksz_ptp_gettime(struct ksz_device dev, struct timespec64 ts)
	{		{
	u32 nanoseconds;		u32 nanoseconds;
	@@ -488,7 +577,29 @@ void ksz_ptp_clock_unregister(struct dsa_switch *ds)

	static irqreturn_t ksz_ptp_msg_thread_fn(int irq, void *dev_id)		static irqreturn_t ksz_ptp_msg_thread_fn(int irq, void *dev_id)
	{		{
			struct ksz_ptp_irq *ptpmsg_irq = dev_id;
			struct ksz_device *dev;
			struct ksz_port *port;
			u32 tstamp_raw;
			ktime_t tstamp;
			int ret;

			port = ptpmsg_irq->port;
			dev = port->ksz_dev;

			if (ptpmsg_irq->ts_en) {
			ret = ksz_read32(dev, ptpmsg_irq->ts_reg, &tstamp_raw);
			if (ret)
	return IRQ_NONE;		return IRQ_NONE;

			tstamp = ksz_decode_tstamp(tstamp_raw);

			port->tstamp_msg = ksz_tstamp_reconstruct(dev, tstamp);

			complete(&port->tstamp_msg_comp);
			}

			return IRQ_HANDLED;
	}		}

	static irqreturn_t ksz_ptp_irq_thread_fn(int irq, void *dev_id)		static irqreturn_t ksz_ptp_irq_thread_fn(int irq, void *dev_id)
	@@ -633,6 +744,8 @@ int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
	REG_PTP_PORT_TX_INT_STATUS__2);		REG_PTP_PORT_TX_INT_STATUS__2);
	snprintf(ptpirq->name, sizeof(ptpirq->name), "ptp-irq-%d", p);		snprintf(ptpirq->name, sizeof(ptpirq->name), "ptp-irq-%d", p);

			init_completion(&port->tstamp_msg_comp);

	ptpirq->domain = irq_domain_add_linear(dev->dev->of_node, ptpirq->nirqs,		ptpirq->domain = irq_domain_add_linear(dev->dev->of_node, ptpirq->nirqs,
	&ksz_ptp_irq_domain_ops, ptpirq);		&ksz_ptp_irq_domain_ops, ptpirq);
	if (!ptpirq->domain)		if (!ptpirq->domain)

drivers/net/dsa/microchip/ksz_ptp.h

+6 −0

Original line number	Original line	Diff line number	Diff line
	@@ -30,6 +30,8 @@ int ksz_get_ts_info(struct dsa_switch *ds, int port,
	struct ethtool_ts_info *ts);		struct ethtool_ts_info *ts);
	int ksz_hwtstamp_get(struct dsa_switch ds, int port, struct ifreq ifr);		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_hwtstamp_set(struct dsa_switch ds, int port, struct ifreq ifr);
			void ksz_port_txtstamp(struct dsa_switch ds, int port, struct sk_buff skb);
			void ksz_port_deferred_xmit(struct kthread_work *work);
	bool ksz_port_rxtstamp(struct dsa_switch ds, int port, struct sk_buff skb,		bool ksz_port_rxtstamp(struct dsa_switch ds, int port, struct sk_buff skb,
	unsigned int type);		unsigned int type);
	int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p);		int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p);
	@@ -64,6 +66,10 @@ static inline void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) {}

	#define ksz_port_rxtstamp NULL		#define ksz_port_rxtstamp NULL

			#define ksz_port_txtstamp NULL

			#define ksz_port_deferred_xmit NULL

	#endif /* End of CONFIG_NET_DSA_MICROCHIP_KSZ_PTP */		#endif /* End of CONFIG_NET_DSA_MICROCHIP_KSZ_PTP */

	#endif		#endif

include/linux/dsa/ksz_common.h

+8 −0

Original line number	Original line	Diff line number	Diff line
	@@ -23,11 +23,19 @@ static inline ktime_t ksz_decode_tstamp(u32 tstamp)
	return ns_to_ktime(ns);		return ns_to_ktime(ns);
	}		}

			struct ksz_deferred_xmit_work {
			struct dsa_port *dp;
			struct sk_buff *skb;
			struct kthread_work work;
			};

	struct ksz_tagger_data {		struct ksz_tagger_data {
			void (xmit_work_fn)(struct kthread_work work);
	void (hwtstamp_set_state)(struct dsa_switch ds, bool on);		void (hwtstamp_set_state)(struct dsa_switch ds, bool on);
	};		};

	struct ksz_skb_cb {		struct ksz_skb_cb {
			struct sk_buff *clone;
	u32 tstamp;		u32 tstamp;
	};		};