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