xref: /openbmc/linux/drivers/ptp/ptp_ines.c (revision 27ab1c1c)
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 
23 MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core");
24 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
25 MODULE_VERSION("1.0");
26 MODULE_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 
111 static LIST_HEAD(ines_clocks);
112 static DEFINE_MUTEX(ines_clocks_lock);
113 
114 struct 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 
125 struct 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 
134 struct 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 
145 struct 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 
161 struct 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 
170 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
171 		       struct ines_timestamp *ts, struct device *dev);
172 static int ines_rxfifo_read(struct ines_port *port);
173 static u64 ines_rxts64(struct ines_port *port, unsigned int words);
174 static bool ines_timestamp_expired(struct ines_timestamp *ts);
175 static u64 ines_txts64(struct ines_port *port, unsigned int words);
176 static void ines_txtstamp_work(struct work_struct *work);
177 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);
178 static u8 tag_to_msgtype(u8 tag);
179 
180 static 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 
191 static 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 
236 static 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 
254 static 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 
285 static 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 
331 static int ines_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
332 {
333 	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
334 	u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;
335 	struct hwtstamp_config cfg;
336 	unsigned long flags;
337 
338 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
339 		return -EFAULT;
340 
341 	/* reserved for future extensions */
342 	if (cfg.flags)
343 		return -EINVAL;
344 
345 	switch (cfg.tx_type) {
346 	case HWTSTAMP_TX_OFF:
347 		ts_stat_tx = 0;
348 		break;
349 	case HWTSTAMP_TX_ON:
350 		ts_stat_tx = TS_ENABLE;
351 		break;
352 	case HWTSTAMP_TX_ONESTEP_P2P:
353 		ts_stat_tx = TS_ENABLE;
354 		cm_one_step = CM_ONE_STEP;
355 		break;
356 	default:
357 		return -ERANGE;
358 	}
359 
360 	switch (cfg.rx_filter) {
361 	case HWTSTAMP_FILTER_NONE:
362 		ts_stat_rx = 0;
363 		break;
364 	case HWTSTAMP_FILTER_ALL:
365 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
366 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
367 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
368 		return -ERANGE;
369 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
370 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
371 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
372 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
373 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
374 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
375 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
376 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
377 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
378 		ts_stat_rx = TS_ENABLE;
379 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
380 		break;
381 	default:
382 		return -ERANGE;
383 	}
384 
385 	spin_lock_irqsave(&port->lock, flags);
386 
387 	port_conf = ines_read32(port, port_conf);
388 	port_conf &= ~CM_ONE_STEP;
389 	port_conf |= cm_one_step;
390 
391 	ines_write32(port, port_conf, port_conf);
392 	ines_write32(port, ts_stat_rx, ts_stat_rx);
393 	ines_write32(port, ts_stat_tx, ts_stat_tx);
394 
395 	port->rxts_enabled = ts_stat_rx == TS_ENABLE;
396 	port->txts_enabled = ts_stat_tx == TS_ENABLE;
397 
398 	spin_unlock_irqrestore(&port->lock, flags);
399 
400 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
401 }
402 
403 static void ines_link_state(struct mii_timestamper *mii_ts,
404 			    struct phy_device *phydev)
405 {
406 	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
407 	u32 port_conf, speed_conf;
408 	unsigned long flags;
409 
410 	switch (phydev->speed) {
411 	case SPEED_10:
412 		speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;
413 		break;
414 	case SPEED_100:
415 		speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;
416 		break;
417 	case SPEED_1000:
418 		speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;
419 		break;
420 	default:
421 		dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);
422 		return;
423 	}
424 	spin_lock_irqsave(&port->lock, flags);
425 
426 	port_conf = ines_read32(port, port_conf);
427 	port_conf &= ~(0x3 << PHY_SPEED_SHIFT);
428 	port_conf |= speed_conf;
429 
430 	ines_write32(port, port_conf, port_conf);
431 
432 	spin_unlock_irqrestore(&port->lock, flags);
433 }
434 
435 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
436 		       struct ines_timestamp *ts, struct device *dev)
437 {
438 	struct ptp_header *hdr;
439 	u16 portn, seqid;
440 	u8 msgtype;
441 	u64 clkid;
442 
443 	if (unlikely(ptp_class & PTP_CLASS_V1))
444 		return false;
445 
446 	hdr = ptp_parse_header(skb, ptp_class);
447 	if (!hdr)
448 		return false;
449 
450 	msgtype = ptp_get_msgtype(hdr, ptp_class);
451 	clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]);
452 	portn = be16_to_cpu(hdr->source_port_identity.port_number);
453 	seqid = be16_to_cpu(hdr->sequence_id);
454 
455 	if (tag_to_msgtype(ts->tag & 0x7) != msgtype) {
456 		dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",
457 			tag_to_msgtype(ts->tag & 0x7), msgtype);
458 		return false;
459 	}
460 	if (ts->clkid != clkid) {
461 		dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",
462 			ts->clkid, clkid);
463 		return false;
464 	}
465 	if (ts->portnum != portn) {
466 		dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",
467 			ts->portnum, portn);
468 		return false;
469 	}
470 	if (ts->seqid != seqid) {
471 		dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",
472 			ts->seqid, seqid);
473 		return false;
474 	}
475 
476 	return true;
477 }
478 
479 static bool ines_rxtstamp(struct mii_timestamper *mii_ts,
480 			  struct sk_buff *skb, int type)
481 {
482 	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
483 	struct skb_shared_hwtstamps *ssh;
484 	u64 ns;
485 
486 	if (!port->rxts_enabled)
487 		return false;
488 
489 	ns = ines_find_rxts(port, skb, type);
490 	if (!ns)
491 		return false;
492 
493 	ssh = skb_hwtstamps(skb);
494 	ssh->hwtstamp = ns_to_ktime(ns);
495 	netif_rx(skb);
496 
497 	return true;
498 }
499 
500 static int ines_rxfifo_read(struct ines_port *port)
501 {
502 	u32 data_rd_pos, buf_stat, mask, ts_stat_rx;
503 	struct ines_timestamp *ts;
504 	unsigned int i;
505 
506 	mask = RX_FIFO_NE_1 << port->index;
507 
508 	for (i = 0; i < INES_FIFO_DEPTH; i++) {
509 		if (list_empty(&port->pool)) {
510 			dev_err(port->clock->dev, "event pool is empty\n");
511 			return -1;
512 		}
513 		buf_stat = ines_read32(port->clock, buf_stat);
514 		if (!(buf_stat & mask))
515 			break;
516 
517 		ts_stat_rx = ines_read32(port, ts_stat_rx);
518 		data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &
519 			DATA_READ_POS_MASK;
520 		if (data_rd_pos) {
521 			dev_err(port->clock->dev, "unexpected Rx read pos %u\n",
522 				data_rd_pos);
523 			break;
524 		}
525 
526 		ts = list_first_entry(&port->pool, struct ines_timestamp, list);
527 		ts->tmo     = jiffies + HZ;
528 		ts->tag     = ines_read32(port, ts_rx);
529 		ts->sec     = ines_rxts64(port, 3);
530 		ts->nsec    = ines_rxts64(port, 2);
531 		ts->clkid   = ines_rxts64(port, 4);
532 		ts->portnum = ines_read32(port, ts_rx);
533 		ts->seqid   = ines_read32(port, ts_rx);
534 
535 		list_del_init(&ts->list);
536 		list_add_tail(&ts->list, &port->events);
537 	}
538 
539 	return 0;
540 }
541 
542 static u64 ines_rxts64(struct ines_port *port, unsigned int words)
543 {
544 	unsigned int i;
545 	u64 result;
546 	u16 word;
547 
548 	word = ines_read32(port, ts_rx);
549 	result = word;
550 	words--;
551 	for (i = 0; i < words; i++) {
552 		word = ines_read32(port, ts_rx);
553 		result <<= 16;
554 		result |= word;
555 	}
556 	return result;
557 }
558 
559 static bool ines_timestamp_expired(struct ines_timestamp *ts)
560 {
561 	return time_after(jiffies, ts->tmo);
562 }
563 
564 static int ines_ts_info(struct mii_timestamper *mii_ts,
565 			struct ethtool_ts_info *info)
566 {
567 	info->so_timestamping =
568 		SOF_TIMESTAMPING_TX_HARDWARE |
569 		SOF_TIMESTAMPING_TX_SOFTWARE |
570 		SOF_TIMESTAMPING_RX_HARDWARE |
571 		SOF_TIMESTAMPING_RX_SOFTWARE |
572 		SOF_TIMESTAMPING_SOFTWARE |
573 		SOF_TIMESTAMPING_RAW_HARDWARE;
574 
575 	info->phc_index = -1;
576 
577 	info->tx_types =
578 		(1 << HWTSTAMP_TX_OFF) |
579 		(1 << HWTSTAMP_TX_ON) |
580 		(1 << HWTSTAMP_TX_ONESTEP_P2P);
581 
582 	info->rx_filters =
583 		(1 << HWTSTAMP_FILTER_NONE) |
584 		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
585 
586 	return 0;
587 }
588 
589 static u64 ines_txts64(struct ines_port *port, unsigned int words)
590 {
591 	unsigned int i;
592 	u64 result;
593 	u16 word;
594 
595 	word = ines_read32(port, ts_tx);
596 	result = word;
597 	words--;
598 	for (i = 0; i < words; i++) {
599 		word = ines_read32(port, ts_tx);
600 		result <<= 16;
601 		result |= word;
602 	}
603 	return result;
604 }
605 
606 static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)
607 {
608 	unsigned long flags;
609 	u32 port_conf;
610 
611 	spin_lock_irqsave(&port->lock, flags);
612 	port_conf = ines_read32(port, port_conf);
613 	spin_unlock_irqrestore(&port->lock, flags);
614 
615 	if (port_conf & CM_ONE_STEP)
616 		return is_sync_pdelay_resp(skb, type);
617 
618 	return false;
619 }
620 
621 static void ines_txtstamp(struct mii_timestamper *mii_ts,
622 			  struct sk_buff *skb, int type)
623 {
624 	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
625 	struct sk_buff *old_skb = NULL;
626 	unsigned long flags;
627 
628 	if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {
629 		kfree_skb(skb);
630 		return;
631 	}
632 
633 	spin_lock_irqsave(&port->lock, flags);
634 
635 	if (port->tx_skb)
636 		old_skb = port->tx_skb;
637 
638 	port->tx_skb = skb;
639 
640 	spin_unlock_irqrestore(&port->lock, flags);
641 
642 	kfree_skb(old_skb);
643 
644 	schedule_delayed_work(&port->ts_work, 1);
645 }
646 
647 static void ines_txtstamp_work(struct work_struct *work)
648 {
649 	struct ines_port *port =
650 		container_of(work, struct ines_port, ts_work.work);
651 	struct skb_shared_hwtstamps ssh;
652 	struct sk_buff *skb;
653 	unsigned long flags;
654 	u64 ns;
655 
656 	spin_lock_irqsave(&port->lock, flags);
657 	skb = port->tx_skb;
658 	port->tx_skb = NULL;
659 	spin_unlock_irqrestore(&port->lock, flags);
660 
661 	ns = ines_find_txts(port, skb);
662 	if (!ns) {
663 		kfree_skb(skb);
664 		return;
665 	}
666 	ssh.hwtstamp = ns_to_ktime(ns);
667 	skb_complete_tx_timestamp(skb, &ssh);
668 }
669 
670 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)
671 {
672 	struct ptp_header *hdr;
673 	u8 msgtype;
674 
675 	hdr = ptp_parse_header(skb, type);
676 	if (!hdr)
677 		return false;
678 
679 	msgtype = ptp_get_msgtype(hdr, type);
680 
681 	switch (msgtype) {
682 	case PTP_MSGTYPE_SYNC:
683 	case PTP_MSGTYPE_PDELAY_RESP:
684 		return true;
685 	default:
686 		return false;
687 	}
688 }
689 
690 static u8 tag_to_msgtype(u8 tag)
691 {
692 	switch (tag) {
693 	case MESSAGE_TYPE_SYNC:
694 		return PTP_MSGTYPE_SYNC;
695 	case MESSAGE_TYPE_P_DELAY_REQ:
696 		return PTP_MSGTYPE_PDELAY_REQ;
697 	case MESSAGE_TYPE_P_DELAY_RESP:
698 		return PTP_MSGTYPE_PDELAY_RESP;
699 	case MESSAGE_TYPE_DELAY_REQ:
700 		return PTP_MSGTYPE_DELAY_REQ;
701 	}
702 	return 0xf;
703 }
704 
705 static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,
706 						      unsigned int index)
707 {
708 	struct device_node *node = device->of_node;
709 	struct ines_port *port;
710 
711 	if (index > INES_N_PORTS - 1) {
712 		dev_err(device, "bad port index %u\n", index);
713 		return ERR_PTR(-EINVAL);
714 	}
715 	port = ines_find_port(node, index);
716 	if (!port) {
717 		dev_err(device, "missing port index %u\n", index);
718 		return ERR_PTR(-ENODEV);
719 	}
720 	port->mii_ts.rxtstamp = ines_rxtstamp;
721 	port->mii_ts.txtstamp = ines_txtstamp;
722 	port->mii_ts.hwtstamp = ines_hwtstamp;
723 	port->mii_ts.link_state = ines_link_state;
724 	port->mii_ts.ts_info = ines_ts_info;
725 
726 	return &port->mii_ts;
727 }
728 
729 static void ines_ptp_release_channel(struct device *device,
730 				     struct mii_timestamper *mii_ts)
731 {
732 }
733 
734 static struct mii_timestamping_ctrl ines_ctrl = {
735 	.probe_channel = ines_ptp_probe_channel,
736 	.release_channel = ines_ptp_release_channel,
737 };
738 
739 static int ines_ptp_ctrl_probe(struct platform_device *pld)
740 {
741 	struct ines_clock *clock;
742 	void __iomem *addr;
743 	int err = 0;
744 
745 	addr = devm_platform_ioremap_resource(pld, 0);
746 	if (IS_ERR(addr)) {
747 		err = PTR_ERR(addr);
748 		goto out;
749 	}
750 	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
751 	if (!clock) {
752 		err = -ENOMEM;
753 		goto out;
754 	}
755 	if (ines_clock_init(clock, &pld->dev, addr)) {
756 		kfree(clock);
757 		err = -ENOMEM;
758 		goto out;
759 	}
760 	err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);
761 	if (err) {
762 		kfree(clock);
763 		goto out;
764 	}
765 	mutex_lock(&ines_clocks_lock);
766 	list_add_tail(&ines_clocks, &clock->list);
767 	mutex_unlock(&ines_clocks_lock);
768 
769 	dev_set_drvdata(&pld->dev, clock);
770 out:
771 	return err;
772 }
773 
774 static int ines_ptp_ctrl_remove(struct platform_device *pld)
775 {
776 	struct ines_clock *clock = dev_get_drvdata(&pld->dev);
777 
778 	unregister_mii_tstamp_controller(&pld->dev);
779 	mutex_lock(&ines_clocks_lock);
780 	list_del(&clock->list);
781 	mutex_unlock(&ines_clocks_lock);
782 	ines_clock_cleanup(clock);
783 	kfree(clock);
784 	return 0;
785 }
786 
787 static const struct of_device_id ines_ptp_ctrl_of_match[] = {
788 	{ .compatible = "ines,ptp-ctrl" },
789 	{ }
790 };
791 
792 MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
793 
794 static struct platform_driver ines_ptp_ctrl_driver = {
795 	.probe  = ines_ptp_ctrl_probe,
796 	.remove = ines_ptp_ctrl_remove,
797 	.driver = {
798 		.name = "ines_ptp_ctrl",
799 		.of_match_table = of_match_ptr(ines_ptp_ctrl_of_match),
800 	},
801 };
802 module_platform_driver(ines_ptp_ctrl_driver);
803