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