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