projects/linux·blob master · refs

814 lines · 19.2 KiB

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 <[email protected]>");
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_obj(*clock);
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);