ptp_ines.c

  1// SPDX-License-Identifier: GPL-2.0
  2//
  3// Copyright (C) 2018 MOSER-BAER AG
  4//
  5
  6#define pr_fmt(fmt) "InES_PTP: " fmt
  7
  8#include <linux/ethtool.h>
  9#include <linux/export.h>
 10#include <linux/if_vlan.h>
 11#include <linux/mii_timestamper.h>
 12#include <linux/module.h>
 13#include <linux/net_tstamp.h>
 14#include <linux/of.h>
 15#include <linux/of_address.h>
 16#include <linux/of_irq.h>
 17#include <linux/phy.h>
 18#include <linux/platform_device.h>
 19#include <linux/ptp_classify.h>
 20#include <linux/ptp_clock_kernel.h>
 21#include <linux/stddef.h>
 22
 23MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core");
 24MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
 25MODULE_VERSION("1.0");
 26MODULE_LICENSE("GPL");
 27
 28/* GLOBAL register */
 29#define MCAST_MAC_SELECT_SHIFT	2
 30#define MCAST_MAC_SELECT_MASK	0x3
 31#define IO_RESET		BIT(1)
 32#define PTP_RESET		BIT(0)
 33
 34/* VERSION register */
 35#define IF_MAJOR_VER_SHIFT	12
 36#define IF_MAJOR_VER_MASK	0xf
 37#define IF_MINOR_VER_SHIFT	8
 38#define IF_MINOR_VER_MASK	0xf
 39#define FPGA_MAJOR_VER_SHIFT	4
 40#define FPGA_MAJOR_VER_MASK	0xf
 41#define FPGA_MINOR_VER_SHIFT	0
 42#define FPGA_MINOR_VER_MASK	0xf
 43
 44/* INT_STAT register */
 45#define RX_INTR_STATUS_3	BIT(5)
 46#define RX_INTR_STATUS_2	BIT(4)
 47#define RX_INTR_STATUS_1	BIT(3)
 48#define TX_INTR_STATUS_3	BIT(2)
 49#define TX_INTR_STATUS_2	BIT(1)
 50#define TX_INTR_STATUS_1	BIT(0)
 51
 52/* INT_MSK register */
 53#define RX_INTR_MASK_3		BIT(5)
 54#define RX_INTR_MASK_2		BIT(4)
 55#define RX_INTR_MASK_1		BIT(3)
 56#define TX_INTR_MASK_3		BIT(2)
 57#define TX_INTR_MASK_2		BIT(1)
 58#define TX_INTR_MASK_1		BIT(0)
 59
 60/* BUF_STAT register */
 61#define RX_FIFO_NE_3		BIT(5)
 62#define RX_FIFO_NE_2		BIT(4)
 63#define RX_FIFO_NE_1		BIT(3)
 64#define TX_FIFO_NE_3		BIT(2)
 65#define TX_FIFO_NE_2		BIT(1)
 66#define TX_FIFO_NE_1		BIT(0)
 67
 68/* PORT_CONF register */
 69#define CM_ONE_STEP		BIT(6)
 70#define PHY_SPEED_SHIFT		4
 71#define PHY_SPEED_MASK		0x3
 72#define P2P_DELAY_WR_POS_SHIFT	2
 73#define P2P_DELAY_WR_POS_MASK	0x3
 74#define PTP_MODE_SHIFT		0
 75#define PTP_MODE_MASK		0x3
 76
 77/* TS_STAT_TX register */
 78#define TS_ENABLE		BIT(15)
 79#define DATA_READ_POS_SHIFT	8
 80#define DATA_READ_POS_MASK	0x1f
 81#define DISCARDED_EVENTS_SHIFT	4
 82#define DISCARDED_EVENTS_MASK	0xf
 83
 84#define INES_N_PORTS		3
 85#define INES_REGISTER_SIZE	0x80
 86#define INES_PORT_OFFSET	0x20
 87#define INES_PORT_SIZE		0x20
 88#define INES_FIFO_DEPTH		90
 89#define INES_MAX_EVENTS		100
 90
 91#define BC_PTP_V1		0
 92#define BC_PTP_V2		1
 93#define TC_E2E_PTP_V2		2
 94#define TC_P2P_PTP_V2		3
 95
 96#define PHY_SPEED_10		0
 97#define PHY_SPEED_100		1
 98#define PHY_SPEED_1000		2
 99
100#define PORT_CONF \
101	((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT))
102
103#define ines_read32(s, r)	__raw_readl((void __iomem *)&s->regs->r)
104#define ines_write32(s, v, r)	__raw_writel(v, (void __iomem *)&s->regs->r)
105
106#define MESSAGE_TYPE_SYNC		1
107#define MESSAGE_TYPE_P_DELAY_REQ	2
108#define MESSAGE_TYPE_P_DELAY_RESP	3
109#define MESSAGE_TYPE_DELAY_REQ		4
110
111static LIST_HEAD(ines_clocks);
112static DEFINE_MUTEX(ines_clocks_lock);
113
114struct ines_global_regs {
115	u32 id;
116	u32 test;
117	u32 global;
118	u32 version;
119	u32 test2;
120	u32 int_stat;
121	u32 int_msk;
122	u32 buf_stat;
123};
124
125struct ines_port_registers {
126	u32 port_conf;
127	u32 p_delay;
128	u32 ts_stat_tx;
129	u32 ts_stat_rx;
130	u32 ts_tx;
131	u32 ts_rx;
132};
133
134struct ines_timestamp {
135	struct list_head list;
136	unsigned long	tmo;
137	u16		tag;
138	u64		sec;
139	u64		nsec;
140	u64		clkid;
141	u16		portnum;
142	u16		seqid;
143};
144
145struct ines_port {
146	struct ines_port_registers	*regs;
147	struct mii_timestamper		mii_ts;
148	struct ines_clock		*clock;
149	bool				rxts_enabled;
150	bool				txts_enabled;
151	unsigned int			index;
152	struct delayed_work		ts_work;
153	/* lock protects event list and tx_skb */
154	spinlock_t			lock;
155	struct sk_buff			*tx_skb;
156	struct list_head		events;
157	struct list_head		pool;
158	struct ines_timestamp		pool_data[INES_MAX_EVENTS];
159};
160
161struct ines_clock {
162	struct ines_port		port[INES_N_PORTS];
163	struct ines_global_regs __iomem	*regs;
164	void __iomem			*base;
165	struct device_node		*node;
166	struct device			*dev;
167	struct list_head		list;
168};
169
170static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
171		       struct ines_timestamp *ts, struct device *dev);
172static int ines_rxfifo_read(struct ines_port *port);
173static u64 ines_rxts64(struct ines_port *port, unsigned int words);
174static bool ines_timestamp_expired(struct ines_timestamp *ts);
175static u64 ines_txts64(struct ines_port *port, unsigned int words);
176static void ines_txtstamp_work(struct work_struct *work);
177static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);
178static u8 tag_to_msgtype(u8 tag);
179
180static void ines_clock_cleanup(struct ines_clock *clock)
181{
182	struct ines_port *port;
183	int i;
184
185	for (i = 0; i < INES_N_PORTS; i++) {
186		port = &clock->port[i];
187		cancel_delayed_work_sync(&port->ts_work);
188	}
189}
190
191static int ines_clock_init(struct ines_clock *clock, struct device *device,
192			   void __iomem *addr)
193{
194	struct device_node *node = device->of_node;
195	unsigned long port_addr;
196	struct ines_port *port;
197	int i, j;
198
199	INIT_LIST_HEAD(&clock->list);
200	clock->node = node;
201	clock->dev  = device;
202	clock->base = addr;
203	clock->regs = clock->base;
204
205	for (i = 0; i < INES_N_PORTS; i++) {
206		port = &clock->port[i];
207		port_addr = (unsigned long) clock->base +
208			INES_PORT_OFFSET + i * INES_PORT_SIZE;
209		port->regs = (struct ines_port_registers *) port_addr;
210		port->clock = clock;
211		port->index = i;
212		INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work);
213		spin_lock_init(&port->lock);
214		INIT_LIST_HEAD(&port->events);
215		INIT_LIST_HEAD(&port->pool);
216		for (j = 0; j < INES_MAX_EVENTS; j++)
217			list_add(&port->pool_data[j].list, &port->pool);
218	}
219
220	ines_write32(clock, 0xBEEF, test);
221	ines_write32(clock, 0xBEEF, test2);
222
223	dev_dbg(device, "ID      0x%x\n", ines_read32(clock, id));
224	dev_dbg(device, "TEST    0x%x\n", ines_read32(clock, test));
225	dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version));
226	dev_dbg(device, "TEST2   0x%x\n", ines_read32(clock, test2));
227
228	for (i = 0; i < INES_N_PORTS; i++) {
229		port = &clock->port[i];
230		ines_write32(port, PORT_CONF, port_conf);
231	}
232
233	return 0;
234}
235
236static struct ines_port *ines_find_port(struct device_node *node, u32 index)
237{
238	struct ines_port *port = NULL;
239	struct ines_clock *clock;
240	struct list_head *this;
241
242	mutex_lock(&ines_clocks_lock);
243	list_for_each(this, &ines_clocks) {
244		clock = list_entry(this, struct ines_clock, list);
245		if (clock->node == node) {
246			port = &clock->port[index];
247			break;
248		}
249	}
250	mutex_unlock(&ines_clocks_lock);
251	return port;
252}
253
254static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type)
255{
256	struct list_head *this, *next;
257	struct ines_timestamp *ts;
258	unsigned long flags;
259	u64 ns = 0;
260
261	if (type == PTP_CLASS_NONE)
262		return 0;
263
264	spin_lock_irqsave(&port->lock, flags);
265	ines_rxfifo_read(port);
266	list_for_each_safe(this, next, &port->events) {
267		ts = list_entry(this, struct ines_timestamp, list);
268		if (ines_timestamp_expired(ts)) {
269			list_del_init(&ts->list);
270			list_add(&ts->list, &port->pool);
271			continue;
272		}
273		if (ines_match(skb, type, ts, port->clock->dev)) {
274			ns = ts->sec * 1000000000ULL + ts->nsec;
275			list_del_init(&ts->list);
276			list_add(&ts->list, &port->pool);
277			break;
278		}
279	}
280	spin_unlock_irqrestore(&port->lock, flags);
281
282	return ns;
283}
284
285static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb)
286{
287	unsigned int class = ptp_classify_raw(skb), i;
288	u32 data_rd_pos, buf_stat, mask, ts_stat_tx;
289	struct ines_timestamp ts;
290	unsigned long flags;
291	u64 ns = 0;
292
293	mask = TX_FIFO_NE_1 << port->index;
294
295	spin_lock_irqsave(&port->lock, flags);
296
297	for (i = 0; i < INES_FIFO_DEPTH; i++) {
298
299		buf_stat = ines_read32(port->clock, buf_stat);
300		if (!(buf_stat & mask)) {
301			dev_dbg(port->clock->dev,
302				  "Tx timestamp FIFO unexpectedly empty\n");
303			break;
304		}
305		ts_stat_tx = ines_read32(port, ts_stat_tx);
306		data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) &
307			DATA_READ_POS_MASK;
308		if (data_rd_pos) {
309			dev_err(port->clock->dev,
310				"unexpected Tx read pos %u\n", data_rd_pos);
311			break;
312		}
313
314		ts.tag     = ines_read32(port, ts_tx);
315		ts.sec     = ines_txts64(port, 3);
316		ts.nsec    = ines_txts64(port, 2);
317		ts.clkid   = ines_txts64(port, 4);
318		ts.portnum = ines_read32(port, ts_tx);
319		ts.seqid   = ines_read32(port, ts_tx);
320
321		if (ines_match(skb, class, &ts, port->clock->dev)) {
322			ns = ts.sec * 1000000000ULL + ts.nsec;
323			break;
324		}
325	}
326
327	spin_unlock_irqrestore(&port->lock, flags);
328	return ns;
329}
330
331static int ines_hwtstamp_get(struct mii_timestamper *mii_ts,
332			     struct kernel_hwtstamp_config *cfg)
333{
334	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
335	unsigned long flags;
336	u32 port_conf;
337
338	cfg->rx_filter = port->rxts_enabled ? HWTSTAMP_FILTER_PTP_V2_EVENT
339					    : HWTSTAMP_FILTER_NONE;
340	if (port->txts_enabled) {
341		spin_lock_irqsave(&port->lock, flags);
342		port_conf = ines_read32(port, port_conf);
343		spin_unlock_irqrestore(&port->lock, flags);
344		cfg->tx_type = (port_conf & CM_ONE_STEP) ? HWTSTAMP_TX_ONESTEP_P2P
345							 : HWTSTAMP_TX_OFF;
346	} else {
347		cfg->tx_type = HWTSTAMP_TX_OFF;
348	}
349
350	return 0;
351}
352
353static int ines_hwtstamp_set(struct mii_timestamper *mii_ts,
354			     struct kernel_hwtstamp_config *cfg,
355			     struct netlink_ext_ack *extack)
356{
357	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
358	u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;
359	unsigned long flags;
360
361	switch (cfg->tx_type) {
362	case HWTSTAMP_TX_OFF:
363		ts_stat_tx = 0;
364		break;
365	case HWTSTAMP_TX_ON:
366		ts_stat_tx = TS_ENABLE;
367		break;
368	case HWTSTAMP_TX_ONESTEP_P2P:
369		ts_stat_tx = TS_ENABLE;
370		cm_one_step = CM_ONE_STEP;
371		break;
372	default:
373		return -ERANGE;
374	}
375
376	switch (cfg->rx_filter) {
377	case HWTSTAMP_FILTER_NONE:
378		ts_stat_rx = 0;
379		break;
380	case HWTSTAMP_FILTER_ALL:
381	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
382	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
383	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
384		return -ERANGE;
385	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
386	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
387	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
388	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
389	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
390	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
391	case HWTSTAMP_FILTER_PTP_V2_EVENT:
392	case HWTSTAMP_FILTER_PTP_V2_SYNC:
393	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
394		ts_stat_rx = TS_ENABLE;
395		cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
396		break;
397	default:
398		return -ERANGE;
399	}
400
401	spin_lock_irqsave(&port->lock, flags);
402
403	port_conf = ines_read32(port, port_conf);
404	port_conf &= ~CM_ONE_STEP;
405	port_conf |= cm_one_step;
406
407	ines_write32(port, port_conf, port_conf);
408	ines_write32(port, ts_stat_rx, ts_stat_rx);
409	ines_write32(port, ts_stat_tx, ts_stat_tx);
410
411	port->rxts_enabled = ts_stat_rx == TS_ENABLE;
412	port->txts_enabled = ts_stat_tx == TS_ENABLE;
413
414	spin_unlock_irqrestore(&port->lock, flags);
415
416	return 0;
417}
418
419static void ines_link_state(struct mii_timestamper *mii_ts,
420			    struct phy_device *phydev)
421{
422	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
423	u32 port_conf, speed_conf;
424	unsigned long flags;
425
426	switch (phydev->speed) {
427	case SPEED_10:
428		speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;
429		break;
430	case SPEED_100:
431		speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;
432		break;
433	case SPEED_1000:
434		speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;
435		break;
436	default:
437		dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);
438		return;
439	}
440	spin_lock_irqsave(&port->lock, flags);
441
442	port_conf = ines_read32(port, port_conf);
443	port_conf &= ~(0x3 << PHY_SPEED_SHIFT);
444	port_conf |= speed_conf;
445
446	ines_write32(port, port_conf, port_conf);
447
448	spin_unlock_irqrestore(&port->lock, flags);
449}
450
451static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
452		       struct ines_timestamp *ts, struct device *dev)
453{
454	struct ptp_header *hdr;
455	u16 portn, seqid;
456	u8 msgtype;
457	u64 clkid;
458
459	if (unlikely(ptp_class & PTP_CLASS_V1))
460		return false;
461
462	hdr = ptp_parse_header(skb, ptp_class);
463	if (!hdr)
464		return false;
465
466	msgtype = ptp_get_msgtype(hdr, ptp_class);
467	clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]);
468	portn = be16_to_cpu(hdr->source_port_identity.port_number);
469	seqid = be16_to_cpu(hdr->sequence_id);
470
471	if (tag_to_msgtype(ts->tag & 0x7) != msgtype) {
472		dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",
473			tag_to_msgtype(ts->tag & 0x7), msgtype);
474		return false;
475	}
476	if (ts->clkid != clkid) {
477		dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",
478			ts->clkid, clkid);
479		return false;
480	}
481	if (ts->portnum != portn) {
482		dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",
483			ts->portnum, portn);
484		return false;
485	}
486	if (ts->seqid != seqid) {
487		dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",
488			ts->seqid, seqid);
489		return false;
490	}
491
492	return true;
493}
494
495static bool ines_rxtstamp(struct mii_timestamper *mii_ts,
496			  struct sk_buff *skb, int type)
497{
498	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
499	struct skb_shared_hwtstamps *ssh;
500	u64 ns;
501
502	if (!port->rxts_enabled)
503		return false;
504
505	ns = ines_find_rxts(port, skb, type);
506	if (!ns)
507		return false;
508
509	ssh = skb_hwtstamps(skb);
510	ssh->hwtstamp = ns_to_ktime(ns);
511	netif_rx(skb);
512
513	return true;
514}
515
516static int ines_rxfifo_read(struct ines_port *port)
517{
518	u32 data_rd_pos, buf_stat, mask, ts_stat_rx;
519	struct ines_timestamp *ts;
520	unsigned int i;
521
522	mask = RX_FIFO_NE_1 << port->index;
523
524	for (i = 0; i < INES_FIFO_DEPTH; i++) {
525		if (list_empty(&port->pool)) {
526			dev_err(port->clock->dev, "event pool is empty\n");
527			return -1;
528		}
529		buf_stat = ines_read32(port->clock, buf_stat);
530		if (!(buf_stat & mask))
531			break;
532
533		ts_stat_rx = ines_read32(port, ts_stat_rx);
534		data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &
535			DATA_READ_POS_MASK;
536		if (data_rd_pos) {
537			dev_err(port->clock->dev, "unexpected Rx read pos %u\n",
538				data_rd_pos);
539			break;
540		}
541
542		ts = list_first_entry(&port->pool, struct ines_timestamp, list);
543		ts->tmo     = jiffies + HZ;
544		ts->tag     = ines_read32(port, ts_rx);
545		ts->sec     = ines_rxts64(port, 3);
546		ts->nsec    = ines_rxts64(port, 2);
547		ts->clkid   = ines_rxts64(port, 4);
548		ts->portnum = ines_read32(port, ts_rx);
549		ts->seqid   = ines_read32(port, ts_rx);
550
551		list_del_init(&ts->list);
552		list_add_tail(&ts->list, &port->events);
553	}
554
555	return 0;
556}
557
558static u64 ines_rxts64(struct ines_port *port, unsigned int words)
559{
560	unsigned int i;
561	u64 result;
562	u16 word;
563
564	word = ines_read32(port, ts_rx);
565	result = word;
566	words--;
567	for (i = 0; i < words; i++) {
568		word = ines_read32(port, ts_rx);
569		result <<= 16;
570		result |= word;
571	}
572	return result;
573}
574
575static bool ines_timestamp_expired(struct ines_timestamp *ts)
576{
577	return time_after(jiffies, ts->tmo);
578}
579
580static int ines_ts_info(struct mii_timestamper *mii_ts,
581			struct kernel_ethtool_ts_info *info)
582{
583	info->so_timestamping =
584		SOF_TIMESTAMPING_TX_HARDWARE |
585		SOF_TIMESTAMPING_TX_SOFTWARE |
586		SOF_TIMESTAMPING_RX_HARDWARE |
587		SOF_TIMESTAMPING_RAW_HARDWARE;
588
589	info->tx_types =
590		(1 << HWTSTAMP_TX_OFF) |
591		(1 << HWTSTAMP_TX_ON) |
592		(1 << HWTSTAMP_TX_ONESTEP_P2P);
593
594	info->rx_filters =
595		(1 << HWTSTAMP_FILTER_NONE) |
596		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
597
598	return 0;
599}
600
601static u64 ines_txts64(struct ines_port *port, unsigned int words)
602{
603	unsigned int i;
604	u64 result;
605	u16 word;
606
607	word = ines_read32(port, ts_tx);
608	result = word;
609	words--;
610	for (i = 0; i < words; i++) {
611		word = ines_read32(port, ts_tx);
612		result <<= 16;
613		result |= word;
614	}
615	return result;
616}
617
618static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)
619{
620	unsigned long flags;
621	u32 port_conf;
622
623	spin_lock_irqsave(&port->lock, flags);
624	port_conf = ines_read32(port, port_conf);
625	spin_unlock_irqrestore(&port->lock, flags);
626
627	if (port_conf & CM_ONE_STEP)
628		return is_sync_pdelay_resp(skb, type);
629
630	return false;
631}
632
633static void ines_txtstamp(struct mii_timestamper *mii_ts,
634			  struct sk_buff *skb, int type)
635{
636	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
637	struct sk_buff *old_skb = NULL;
638	unsigned long flags;
639
640	if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {
641		kfree_skb(skb);
642		return;
643	}
644
645	spin_lock_irqsave(&port->lock, flags);
646
647	if (port->tx_skb)
648		old_skb = port->tx_skb;
649
650	port->tx_skb = skb;
651
652	spin_unlock_irqrestore(&port->lock, flags);
653
654	kfree_skb(old_skb);
655
656	schedule_delayed_work(&port->ts_work, 1);
657}
658
659static void ines_txtstamp_work(struct work_struct *work)
660{
661	struct ines_port *port =
662		container_of(work, struct ines_port, ts_work.work);
663	struct skb_shared_hwtstamps ssh;
664	struct sk_buff *skb;
665	unsigned long flags;
666	u64 ns;
667
668	spin_lock_irqsave(&port->lock, flags);
669	skb = port->tx_skb;
670	port->tx_skb = NULL;
671	spin_unlock_irqrestore(&port->lock, flags);
672
673	ns = ines_find_txts(port, skb);
674	if (!ns) {
675		kfree_skb(skb);
676		return;
677	}
678	ssh.hwtstamp = ns_to_ktime(ns);
679	skb_complete_tx_timestamp(skb, &ssh);
680}
681
682static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)
683{
684	struct ptp_header *hdr;
685	u8 msgtype;
686
687	hdr = ptp_parse_header(skb, type);
688	if (!hdr)
689		return false;
690
691	msgtype = ptp_get_msgtype(hdr, type);
692
693	switch (msgtype) {
694	case PTP_MSGTYPE_SYNC:
695	case PTP_MSGTYPE_PDELAY_RESP:
696		return true;
697	default:
698		return false;
699	}
700}
701
702static u8 tag_to_msgtype(u8 tag)
703{
704	switch (tag) {
705	case MESSAGE_TYPE_SYNC:
706		return PTP_MSGTYPE_SYNC;
707	case MESSAGE_TYPE_P_DELAY_REQ:
708		return PTP_MSGTYPE_PDELAY_REQ;
709	case MESSAGE_TYPE_P_DELAY_RESP:
710		return PTP_MSGTYPE_PDELAY_RESP;
711	case MESSAGE_TYPE_DELAY_REQ:
712		return PTP_MSGTYPE_DELAY_REQ;
713	}
714	return 0xf;
715}
716
717static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,
718						      unsigned int index)
719{
720	struct device_node *node = device->of_node;
721	struct ines_port *port;
722
723	if (index > INES_N_PORTS - 1) {
724		dev_err(device, "bad port index %u\n", index);
725		return ERR_PTR(-EINVAL);
726	}
727	port = ines_find_port(node, index);
728	if (!port) {
729		dev_err(device, "missing port index %u\n", index);
730		return ERR_PTR(-ENODEV);
731	}
732	port->mii_ts.rxtstamp = ines_rxtstamp;
733	port->mii_ts.txtstamp = ines_txtstamp;
734	port->mii_ts.hwtstamp_set = ines_hwtstamp_set;
735	port->mii_ts.hwtstamp_get = ines_hwtstamp_get;
736	port->mii_ts.link_state = ines_link_state;
737	port->mii_ts.ts_info = ines_ts_info;
738
739	return &port->mii_ts;
740}
741
742static void ines_ptp_release_channel(struct device *device,
743				     struct mii_timestamper *mii_ts)
744{
745}
746
747static struct mii_timestamping_ctrl ines_ctrl = {
748	.probe_channel = ines_ptp_probe_channel,
749	.release_channel = ines_ptp_release_channel,
750};
751
752static int ines_ptp_ctrl_probe(struct platform_device *pld)
753{
754	struct ines_clock *clock;
755	void __iomem *addr;
756	int err = 0;
757
758	addr = devm_platform_ioremap_resource(pld, 0);
759	if (IS_ERR(addr)) {
760		err = PTR_ERR(addr);
761		goto out;
762	}
763	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
764	if (!clock) {
765		err = -ENOMEM;
766		goto out;
767	}
768	if (ines_clock_init(clock, &pld->dev, addr)) {
769		kfree(clock);
770		err = -ENOMEM;
771		goto out;
772	}
773	err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);
774	if (err) {
775		kfree(clock);
776		goto out;
777	}
778	mutex_lock(&ines_clocks_lock);
779	list_add_tail(&ines_clocks, &clock->list);
780	mutex_unlock(&ines_clocks_lock);
781
782	dev_set_drvdata(&pld->dev, clock);
783out:
784	return err;
785}
786
787static void ines_ptp_ctrl_remove(struct platform_device *pld)
788{
789	struct ines_clock *clock = dev_get_drvdata(&pld->dev);
790
791	unregister_mii_tstamp_controller(&pld->dev);
792	mutex_lock(&ines_clocks_lock);
793	list_del(&clock->list);
794	mutex_unlock(&ines_clocks_lock);
795	ines_clock_cleanup(clock);
796	kfree(clock);
797}
798
799static const struct of_device_id ines_ptp_ctrl_of_match[] = {
800	{ .compatible = "ines,ptp-ctrl" },
801	{ }
802};
803
804MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
805
806static struct platform_driver ines_ptp_ctrl_driver = {
807	.probe  = ines_ptp_ctrl_probe,
808	.remove = ines_ptp_ctrl_remove,
809	.driver = {
810		.name = "ines_ptp_ctrl",
811		.of_match_table = ines_ptp_ctrl_of_match,
812	},
813};
814module_platform_driver(ines_ptp_ctrl_driver);