2 * Driver for the National Semiconductor DP83640 PHYTER
4 * Copyright (C) 2010 OMICRON electronics GmbH
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
21 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
23 #include <linux/ethtool.h>
24 #include <linux/kernel.h>
25 #include <linux/list.h>
26 #include <linux/mii.h>
27 #include <linux/module.h>
28 #include <linux/net_tstamp.h>
29 #include <linux/netdevice.h>
30 #include <linux/phy.h>
31 #include <linux/ptp_classify.h>
32 #include <linux/ptp_clock_kernel.h>
34 #include "dp83640_reg.h"
36 #define DP83640_PHY_ID 0x20005ce1
43 #define PSF_EVNT 0x4000
51 #define MII_DP83640_MICR 0x11
52 #define MII_DP83640_MISR 0x12
54 #define MII_DP83640_MICR_OE 0x1
55 #define MII_DP83640_MICR_IE 0x2
57 #define MII_DP83640_MISR_RHF_INT_EN 0x01
58 #define MII_DP83640_MISR_FHF_INT_EN 0x02
59 #define MII_DP83640_MISR_ANC_INT_EN 0x04
60 #define MII_DP83640_MISR_DUP_INT_EN 0x08
61 #define MII_DP83640_MISR_SPD_INT_EN 0x10
62 #define MII_DP83640_MISR_LINK_INT_EN 0x20
63 #define MII_DP83640_MISR_ED_INT_EN 0x40
64 #define MII_DP83640_MISR_LQ_INT_EN 0x80
66 /* phyter seems to miss the mark by 16 ns */
67 #define ADJTIME_FIX 16
69 #if defined(__BIG_ENDIAN)
71 #elif defined(__LITTLE_ENDIAN)
72 #define ENDIAN_FLAG PSF_ENDIAN
75 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
78 u16 ns_lo
; /* ns[15:0] */
79 u16 ns_hi
; /* overflow[1:0], ns[29:16] */
80 u16 sec_lo
; /* sec[15:0] */
81 u16 sec_hi
; /* sec[31:16] */
82 u16 seqid
; /* sequenceId[15:0] */
83 u16 msgtype
; /* messageType[3:0], hash[11:0] */
87 u16 ns_lo
; /* ns[15:0] */
88 u16 ns_hi
; /* overflow[1:0], ns[29:16] */
89 u16 sec_lo
; /* sec[15:0] */
90 u16 sec_hi
; /* sec[31:16] */
94 struct list_head list
;
102 struct dp83640_clock
;
104 struct dp83640_private
{
105 struct list_head list
;
106 struct dp83640_clock
*clock
;
107 struct phy_device
*phydev
;
108 struct work_struct ts_work
;
113 /* remember state of cfg0 during calibration */
115 /* remember the last event time stamp */
116 struct phy_txts edata
;
117 /* list of rx timestamps */
118 struct list_head rxts
;
119 struct list_head rxpool
;
120 struct rxts rx_pool_data
[MAX_RXTS
];
121 /* protects above three fields from concurrent access */
123 /* queues of incoming and outgoing packets */
124 struct sk_buff_head rx_queue
;
125 struct sk_buff_head tx_queue
;
128 struct dp83640_clock
{
129 /* keeps the instance in the 'phyter_clocks' list */
130 struct list_head list
;
131 /* we create one clock instance per MII bus */
133 /* protects extended registers from concurrent access */
134 struct mutex extreg_lock
;
135 /* remembers which page was last selected */
137 /* our advertised capabilities */
138 struct ptp_clock_info caps
;
139 /* protects the three fields below from concurrent access */
140 struct mutex clock_lock
;
141 /* the one phyter from which we shall read */
142 struct dp83640_private
*chosen
;
143 /* list of the other attached phyters, not chosen */
144 struct list_head phylist
;
145 /* reference to our PTP hardware clock */
146 struct ptp_clock
*ptp_clock
;
163 static int chosen_phy
= -1;
164 static ushort gpio_tab
[GPIO_TABLE_SIZE
] = {
165 1, 2, 3, 4, 8, 9, 10, 11
168 module_param(chosen_phy
, int, 0444);
169 module_param_array(gpio_tab
, ushort
, NULL
, 0444);
171 MODULE_PARM_DESC(chosen_phy
, \
172 "The address of the PHY to use for the ancillary clock features");
173 MODULE_PARM_DESC(gpio_tab
, \
174 "Which GPIO line to use for which purpose: cal,perout,extts1,...,extts6");
176 /* a list of clocks and a mutex to protect it */
177 static LIST_HEAD(phyter_clocks
);
178 static DEFINE_MUTEX(phyter_clocks_lock
);
180 static void rx_timestamp_work(struct work_struct
*work
);
182 /* extended register access functions */
184 #define BROADCAST_ADDR 31
186 static inline int broadcast_write(struct mii_bus
*bus
, u32 regnum
, u16 val
)
188 return mdiobus_write(bus
, BROADCAST_ADDR
, regnum
, val
);
191 /* Caller must hold extreg_lock. */
192 static int ext_read(struct phy_device
*phydev
, int page
, u32 regnum
)
194 struct dp83640_private
*dp83640
= phydev
->priv
;
197 if (dp83640
->clock
->page
!= page
) {
198 broadcast_write(phydev
->bus
, PAGESEL
, page
);
199 dp83640
->clock
->page
= page
;
201 val
= phy_read(phydev
, regnum
);
206 /* Caller must hold extreg_lock. */
207 static void ext_write(int broadcast
, struct phy_device
*phydev
,
208 int page
, u32 regnum
, u16 val
)
210 struct dp83640_private
*dp83640
= phydev
->priv
;
212 if (dp83640
->clock
->page
!= page
) {
213 broadcast_write(phydev
->bus
, PAGESEL
, page
);
214 dp83640
->clock
->page
= page
;
217 broadcast_write(phydev
->bus
, regnum
, val
);
219 phy_write(phydev
, regnum
, val
);
222 /* Caller must hold extreg_lock. */
223 static int tdr_write(int bc
, struct phy_device
*dev
,
224 const struct timespec
*ts
, u16 cmd
)
226 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_nsec
& 0xffff);/* ns[15:0] */
227 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_nsec
>> 16); /* ns[31:16] */
228 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_sec
& 0xffff); /* sec[15:0] */
229 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_sec
>> 16); /* sec[31:16]*/
231 ext_write(bc
, dev
, PAGE4
, PTP_CTL
, cmd
);
236 /* convert phy timestamps into driver timestamps */
238 static void phy2rxts(struct phy_rxts
*p
, struct rxts
*rxts
)
243 sec
|= p
->sec_hi
<< 16;
246 rxts
->ns
|= (p
->ns_hi
& 0x3fff) << 16;
247 rxts
->ns
+= ((u64
)sec
) * 1000000000ULL;
248 rxts
->seqid
= p
->seqid
;
249 rxts
->msgtype
= (p
->msgtype
>> 12) & 0xf;
250 rxts
->hash
= p
->msgtype
& 0x0fff;
251 rxts
->tmo
= jiffies
+ 2;
254 static u64
phy2txts(struct phy_txts
*p
)
260 sec
|= p
->sec_hi
<< 16;
263 ns
|= (p
->ns_hi
& 0x3fff) << 16;
264 ns
+= ((u64
)sec
) * 1000000000ULL;
269 static void periodic_output(struct dp83640_clock
*clock
,
270 struct ptp_clock_request
*clkreq
, bool on
)
272 struct dp83640_private
*dp83640
= clock
->chosen
;
273 struct phy_device
*phydev
= dp83640
->phydev
;
274 u32 sec
, nsec
, period
;
275 u16 gpio
, ptp_trig
, trigger
, val
;
277 gpio
= on
? gpio_tab
[PEROUT_GPIO
] : 0;
278 trigger
= PER_TRIGGER
;
281 (trigger
& TRIG_CSEL_MASK
) << TRIG_CSEL_SHIFT
|
282 (gpio
& TRIG_GPIO_MASK
) << TRIG_GPIO_SHIFT
|
286 val
= (trigger
& TRIG_SEL_MASK
) << TRIG_SEL_SHIFT
;
290 mutex_lock(&clock
->extreg_lock
);
291 ext_write(0, phydev
, PAGE5
, PTP_TRIG
, ptp_trig
);
292 ext_write(0, phydev
, PAGE4
, PTP_CTL
, val
);
293 mutex_unlock(&clock
->extreg_lock
);
297 sec
= clkreq
->perout
.start
.sec
;
298 nsec
= clkreq
->perout
.start
.nsec
;
299 period
= clkreq
->perout
.period
.sec
* 1000000000UL;
300 period
+= clkreq
->perout
.period
.nsec
;
302 mutex_lock(&clock
->extreg_lock
);
304 ext_write(0, phydev
, PAGE5
, PTP_TRIG
, ptp_trig
);
308 ext_write(0, phydev
, PAGE4
, PTP_CTL
, val
);
309 ext_write(0, phydev
, PAGE4
, PTP_TDR
, nsec
& 0xffff); /* ns[15:0] */
310 ext_write(0, phydev
, PAGE4
, PTP_TDR
, nsec
>> 16); /* ns[31:16] */
311 ext_write(0, phydev
, PAGE4
, PTP_TDR
, sec
& 0xffff); /* sec[15:0] */
312 ext_write(0, phydev
, PAGE4
, PTP_TDR
, sec
>> 16); /* sec[31:16] */
313 ext_write(0, phydev
, PAGE4
, PTP_TDR
, period
& 0xffff); /* ns[15:0] */
314 ext_write(0, phydev
, PAGE4
, PTP_TDR
, period
>> 16); /* ns[31:16] */
319 ext_write(0, phydev
, PAGE4
, PTP_CTL
, val
);
321 mutex_unlock(&clock
->extreg_lock
);
324 /* ptp clock methods */
326 static int ptp_dp83640_adjfreq(struct ptp_clock_info
*ptp
, s32 ppb
)
328 struct dp83640_clock
*clock
=
329 container_of(ptp
, struct dp83640_clock
, caps
);
330 struct phy_device
*phydev
= clock
->chosen
->phydev
;
341 rate
= div_u64(rate
, 1953125);
343 hi
= (rate
>> 16) & PTP_RATE_HI_MASK
;
349 mutex_lock(&clock
->extreg_lock
);
351 ext_write(1, phydev
, PAGE4
, PTP_RATEH
, hi
);
352 ext_write(1, phydev
, PAGE4
, PTP_RATEL
, lo
);
354 mutex_unlock(&clock
->extreg_lock
);
359 static int ptp_dp83640_adjtime(struct ptp_clock_info
*ptp
, s64 delta
)
361 struct dp83640_clock
*clock
=
362 container_of(ptp
, struct dp83640_clock
, caps
);
363 struct phy_device
*phydev
= clock
->chosen
->phydev
;
367 delta
+= ADJTIME_FIX
;
369 ts
= ns_to_timespec(delta
);
371 mutex_lock(&clock
->extreg_lock
);
373 err
= tdr_write(1, phydev
, &ts
, PTP_STEP_CLK
);
375 mutex_unlock(&clock
->extreg_lock
);
380 static int ptp_dp83640_gettime(struct ptp_clock_info
*ptp
, struct timespec
*ts
)
382 struct dp83640_clock
*clock
=
383 container_of(ptp
, struct dp83640_clock
, caps
);
384 struct phy_device
*phydev
= clock
->chosen
->phydev
;
387 mutex_lock(&clock
->extreg_lock
);
389 ext_write(0, phydev
, PAGE4
, PTP_CTL
, PTP_RD_CLK
);
391 val
[0] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* ns[15:0] */
392 val
[1] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* ns[31:16] */
393 val
[2] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* sec[15:0] */
394 val
[3] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* sec[31:16] */
396 mutex_unlock(&clock
->extreg_lock
);
398 ts
->tv_nsec
= val
[0] | (val
[1] << 16);
399 ts
->tv_sec
= val
[2] | (val
[3] << 16);
404 static int ptp_dp83640_settime(struct ptp_clock_info
*ptp
,
405 const struct timespec
*ts
)
407 struct dp83640_clock
*clock
=
408 container_of(ptp
, struct dp83640_clock
, caps
);
409 struct phy_device
*phydev
= clock
->chosen
->phydev
;
412 mutex_lock(&clock
->extreg_lock
);
414 err
= tdr_write(1, phydev
, ts
, PTP_LOAD_CLK
);
416 mutex_unlock(&clock
->extreg_lock
);
421 static int ptp_dp83640_enable(struct ptp_clock_info
*ptp
,
422 struct ptp_clock_request
*rq
, int on
)
424 struct dp83640_clock
*clock
=
425 container_of(ptp
, struct dp83640_clock
, caps
);
426 struct phy_device
*phydev
= clock
->chosen
->phydev
;
428 u16 evnt
, event_num
, gpio_num
;
431 case PTP_CLK_REQ_EXTTS
:
432 index
= rq
->extts
.index
;
433 if (index
< 0 || index
>= N_EXT_TS
)
435 event_num
= EXT_EVENT
+ index
;
436 evnt
= EVNT_WR
| (event_num
& EVNT_SEL_MASK
) << EVNT_SEL_SHIFT
;
438 gpio_num
= gpio_tab
[EXTTS0_GPIO
+ index
];
439 evnt
|= (gpio_num
& EVNT_GPIO_MASK
) << EVNT_GPIO_SHIFT
;
442 ext_write(0, phydev
, PAGE5
, PTP_EVNT
, evnt
);
445 case PTP_CLK_REQ_PEROUT
:
446 if (rq
->perout
.index
!= 0)
448 periodic_output(clock
, rq
, on
);
458 static u8 status_frame_dst
[6] = { 0x01, 0x1B, 0x19, 0x00, 0x00, 0x00 };
459 static u8 status_frame_src
[6] = { 0x08, 0x00, 0x17, 0x0B, 0x6B, 0x0F };
461 static void enable_status_frames(struct phy_device
*phydev
, bool on
)
466 cfg0
= PSF_EVNT_EN
| PSF_RXTS_EN
| PSF_TXTS_EN
| ENDIAN_FLAG
;
468 ver
= (PSF_PTPVER
& VERSIONPTP_MASK
) << VERSIONPTP_SHIFT
;
470 ext_write(0, phydev
, PAGE5
, PSF_CFG0
, cfg0
);
471 ext_write(0, phydev
, PAGE6
, PSF_CFG1
, ver
);
473 if (!phydev
->attached_dev
) {
474 pr_warn("expected to find an attached netdevice\n");
479 if (dev_mc_add(phydev
->attached_dev
, status_frame_dst
))
480 pr_warn("failed to add mc address\n");
482 if (dev_mc_del(phydev
->attached_dev
, status_frame_dst
))
483 pr_warn("failed to delete mc address\n");
487 static bool is_status_frame(struct sk_buff
*skb
, int type
)
489 struct ethhdr
*h
= eth_hdr(skb
);
491 if (PTP_CLASS_V2_L2
== type
&&
492 !memcmp(h
->h_source
, status_frame_src
, sizeof(status_frame_src
)))
498 static int expired(struct rxts
*rxts
)
500 return time_after(jiffies
, rxts
->tmo
);
503 /* Caller must hold rx_lock. */
504 static void prune_rx_ts(struct dp83640_private
*dp83640
)
506 struct list_head
*this, *next
;
509 list_for_each_safe(this, next
, &dp83640
->rxts
) {
510 rxts
= list_entry(this, struct rxts
, list
);
512 list_del_init(&rxts
->list
);
513 list_add(&rxts
->list
, &dp83640
->rxpool
);
518 /* synchronize the phyters so they act as one clock */
520 static void enable_broadcast(struct phy_device
*phydev
, int init_page
, int on
)
523 phy_write(phydev
, PAGESEL
, 0);
524 val
= phy_read(phydev
, PHYCR2
);
529 phy_write(phydev
, PHYCR2
, val
);
530 phy_write(phydev
, PAGESEL
, init_page
);
533 static void recalibrate(struct dp83640_clock
*clock
)
536 struct phy_txts event_ts
;
538 struct list_head
*this;
539 struct dp83640_private
*tmp
;
540 struct phy_device
*master
= clock
->chosen
->phydev
;
541 u16 cal_gpio
, cfg0
, evnt
, ptp_trig
, trigger
, val
;
543 trigger
= CAL_TRIGGER
;
544 cal_gpio
= gpio_tab
[CALIBRATE_GPIO
];
546 mutex_lock(&clock
->extreg_lock
);
549 * enable broadcast, disable status frames, enable ptp clock
551 list_for_each(this, &clock
->phylist
) {
552 tmp
= list_entry(this, struct dp83640_private
, list
);
553 enable_broadcast(tmp
->phydev
, clock
->page
, 1);
554 tmp
->cfg0
= ext_read(tmp
->phydev
, PAGE5
, PSF_CFG0
);
555 ext_write(0, tmp
->phydev
, PAGE5
, PSF_CFG0
, 0);
556 ext_write(0, tmp
->phydev
, PAGE4
, PTP_CTL
, PTP_ENABLE
);
558 enable_broadcast(master
, clock
->page
, 1);
559 cfg0
= ext_read(master
, PAGE5
, PSF_CFG0
);
560 ext_write(0, master
, PAGE5
, PSF_CFG0
, 0);
561 ext_write(0, master
, PAGE4
, PTP_CTL
, PTP_ENABLE
);
564 * enable an event timestamp
566 evnt
= EVNT_WR
| EVNT_RISE
| EVNT_SINGLE
;
567 evnt
|= (CAL_EVENT
& EVNT_SEL_MASK
) << EVNT_SEL_SHIFT
;
568 evnt
|= (cal_gpio
& EVNT_GPIO_MASK
) << EVNT_GPIO_SHIFT
;
570 list_for_each(this, &clock
->phylist
) {
571 tmp
= list_entry(this, struct dp83640_private
, list
);
572 ext_write(0, tmp
->phydev
, PAGE5
, PTP_EVNT
, evnt
);
574 ext_write(0, master
, PAGE5
, PTP_EVNT
, evnt
);
577 * configure a trigger
579 ptp_trig
= TRIG_WR
| TRIG_IF_LATE
| TRIG_PULSE
;
580 ptp_trig
|= (trigger
& TRIG_CSEL_MASK
) << TRIG_CSEL_SHIFT
;
581 ptp_trig
|= (cal_gpio
& TRIG_GPIO_MASK
) << TRIG_GPIO_SHIFT
;
582 ext_write(0, master
, PAGE5
, PTP_TRIG
, ptp_trig
);
585 val
= (trigger
& TRIG_SEL_MASK
) << TRIG_SEL_SHIFT
;
587 ext_write(0, master
, PAGE4
, PTP_CTL
, val
);
592 ext_write(0, master
, PAGE4
, PTP_CTL
, val
);
594 /* disable trigger */
595 val
= (trigger
& TRIG_SEL_MASK
) << TRIG_SEL_SHIFT
;
597 ext_write(0, master
, PAGE4
, PTP_CTL
, val
);
600 * read out and correct offsets
602 val
= ext_read(master
, PAGE4
, PTP_STS
);
603 pr_info("master PTP_STS 0x%04hx\n", val
);
604 val
= ext_read(master
, PAGE4
, PTP_ESTS
);
605 pr_info("master PTP_ESTS 0x%04hx\n", val
);
606 event_ts
.ns_lo
= ext_read(master
, PAGE4
, PTP_EDATA
);
607 event_ts
.ns_hi
= ext_read(master
, PAGE4
, PTP_EDATA
);
608 event_ts
.sec_lo
= ext_read(master
, PAGE4
, PTP_EDATA
);
609 event_ts
.sec_hi
= ext_read(master
, PAGE4
, PTP_EDATA
);
610 now
= phy2txts(&event_ts
);
612 list_for_each(this, &clock
->phylist
) {
613 tmp
= list_entry(this, struct dp83640_private
, list
);
614 val
= ext_read(tmp
->phydev
, PAGE4
, PTP_STS
);
615 pr_info("slave PTP_STS 0x%04hx\n", val
);
616 val
= ext_read(tmp
->phydev
, PAGE4
, PTP_ESTS
);
617 pr_info("slave PTP_ESTS 0x%04hx\n", val
);
618 event_ts
.ns_lo
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
619 event_ts
.ns_hi
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
620 event_ts
.sec_lo
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
621 event_ts
.sec_hi
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
622 diff
= now
- (s64
) phy2txts(&event_ts
);
623 pr_info("slave offset %lld nanoseconds\n", diff
);
625 ts
= ns_to_timespec(diff
);
626 tdr_write(0, tmp
->phydev
, &ts
, PTP_STEP_CLK
);
630 * restore status frames
632 list_for_each(this, &clock
->phylist
) {
633 tmp
= list_entry(this, struct dp83640_private
, list
);
634 ext_write(0, tmp
->phydev
, PAGE5
, PSF_CFG0
, tmp
->cfg0
);
636 ext_write(0, master
, PAGE5
, PSF_CFG0
, cfg0
);
638 mutex_unlock(&clock
->extreg_lock
);
641 /* time stamping methods */
643 static inline u16
exts_chan_to_edata(int ch
)
645 return 1 << ((ch
+ EXT_EVENT
) * 2);
648 static int decode_evnt(struct dp83640_private
*dp83640
,
649 void *data
, u16 ests
)
651 struct phy_txts
*phy_txts
;
652 struct ptp_clock_event event
;
654 int words
= (ests
>> EVNT_TS_LEN_SHIFT
) & EVNT_TS_LEN_MASK
;
657 if (ests
& MULT_EVNT
) {
658 ext_status
= *(u16
*) data
;
659 data
+= sizeof(ext_status
);
664 switch (words
) { /* fall through in every case */
666 dp83640
->edata
.sec_hi
= phy_txts
->sec_hi
;
668 dp83640
->edata
.sec_lo
= phy_txts
->sec_lo
;
670 dp83640
->edata
.ns_hi
= phy_txts
->ns_hi
;
672 dp83640
->edata
.ns_lo
= phy_txts
->ns_lo
;
679 i
= ((ests
>> EVNT_NUM_SHIFT
) & EVNT_NUM_MASK
) - EXT_EVENT
;
680 ext_status
= exts_chan_to_edata(i
);
683 event
.type
= PTP_CLOCK_EXTTS
;
684 event
.timestamp
= phy2txts(&dp83640
->edata
);
686 for (i
= 0; i
< N_EXT_TS
; i
++) {
687 if (ext_status
& exts_chan_to_edata(i
)) {
689 ptp_clock_event(dp83640
->clock
->ptp_clock
, &event
);
693 return parsed
* sizeof(u16
);
696 static void decode_rxts(struct dp83640_private
*dp83640
,
697 struct phy_rxts
*phy_rxts
)
702 spin_lock_irqsave(&dp83640
->rx_lock
, flags
);
704 prune_rx_ts(dp83640
);
706 if (list_empty(&dp83640
->rxpool
)) {
707 pr_debug("rx timestamp pool is empty\n");
710 rxts
= list_first_entry(&dp83640
->rxpool
, struct rxts
, list
);
711 list_del_init(&rxts
->list
);
712 phy2rxts(phy_rxts
, rxts
);
713 list_add_tail(&rxts
->list
, &dp83640
->rxts
);
715 spin_unlock_irqrestore(&dp83640
->rx_lock
, flags
);
718 static void decode_txts(struct dp83640_private
*dp83640
,
719 struct phy_txts
*phy_txts
)
721 struct skb_shared_hwtstamps shhwtstamps
;
725 /* We must already have the skb that triggered this. */
727 skb
= skb_dequeue(&dp83640
->tx_queue
);
730 pr_debug("have timestamp but tx_queue empty\n");
733 ns
= phy2txts(phy_txts
);
734 memset(&shhwtstamps
, 0, sizeof(shhwtstamps
));
735 shhwtstamps
.hwtstamp
= ns_to_ktime(ns
);
736 skb_complete_tx_timestamp(skb
, &shhwtstamps
);
739 static void decode_status_frame(struct dp83640_private
*dp83640
,
742 struct phy_rxts
*phy_rxts
;
743 struct phy_txts
*phy_txts
;
750 for (len
= skb_headlen(skb
) - 2; len
> sizeof(type
); len
-= size
) {
753 ests
= type
& 0x0fff;
754 type
= type
& 0xf000;
758 if (PSF_RX
== type
&& len
>= sizeof(*phy_rxts
)) {
760 phy_rxts
= (struct phy_rxts
*) ptr
;
761 decode_rxts(dp83640
, phy_rxts
);
762 size
= sizeof(*phy_rxts
);
764 } else if (PSF_TX
== type
&& len
>= sizeof(*phy_txts
)) {
766 phy_txts
= (struct phy_txts
*) ptr
;
767 decode_txts(dp83640
, phy_txts
);
768 size
= sizeof(*phy_txts
);
770 } else if (PSF_EVNT
== type
&& len
>= sizeof(*phy_txts
)) {
772 size
= decode_evnt(dp83640
, ptr
, ests
);
782 static int is_sync(struct sk_buff
*skb
, int type
)
784 u8
*data
= skb
->data
, *msgtype
;
785 unsigned int offset
= 0;
788 case PTP_CLASS_V1_IPV4
:
789 case PTP_CLASS_V2_IPV4
:
790 offset
= ETH_HLEN
+ IPV4_HLEN(data
) + UDP_HLEN
;
792 case PTP_CLASS_V1_IPV6
:
793 case PTP_CLASS_V2_IPV6
:
796 case PTP_CLASS_V2_L2
:
799 case PTP_CLASS_V2_VLAN
:
800 offset
= ETH_HLEN
+ VLAN_HLEN
;
806 if (type
& PTP_CLASS_V1
)
807 offset
+= OFF_PTP_CONTROL
;
809 if (skb
->len
< offset
+ 1)
812 msgtype
= data
+ offset
;
814 return (*msgtype
& 0xf) == 0;
817 static int match(struct sk_buff
*skb
, unsigned int type
, struct rxts
*rxts
)
821 u8
*msgtype
, *data
= skb_mac_header(skb
);
823 /* check sequenceID, messageType, 12 bit hash of offset 20-29 */
826 case PTP_CLASS_V1_IPV4
:
827 case PTP_CLASS_V2_IPV4
:
828 offset
= ETH_HLEN
+ IPV4_HLEN(data
) + UDP_HLEN
;
830 case PTP_CLASS_V1_IPV6
:
831 case PTP_CLASS_V2_IPV6
:
834 case PTP_CLASS_V2_L2
:
837 case PTP_CLASS_V2_VLAN
:
838 offset
= ETH_HLEN
+ VLAN_HLEN
;
844 if (skb
->len
+ ETH_HLEN
< offset
+ OFF_PTP_SEQUENCE_ID
+ sizeof(*seqid
))
847 if (unlikely(type
& PTP_CLASS_V1
))
848 msgtype
= data
+ offset
+ OFF_PTP_CONTROL
;
850 msgtype
= data
+ offset
;
852 seqid
= (u16
*)(data
+ offset
+ OFF_PTP_SEQUENCE_ID
);
854 return (rxts
->msgtype
== (*msgtype
& 0xf) &&
855 rxts
->seqid
== ntohs(*seqid
));
858 static void dp83640_free_clocks(void)
860 struct dp83640_clock
*clock
;
861 struct list_head
*this, *next
;
863 mutex_lock(&phyter_clocks_lock
);
865 list_for_each_safe(this, next
, &phyter_clocks
) {
866 clock
= list_entry(this, struct dp83640_clock
, list
);
867 if (!list_empty(&clock
->phylist
)) {
868 pr_warn("phy list non-empty while unloading\n");
871 list_del(&clock
->list
);
872 mutex_destroy(&clock
->extreg_lock
);
873 mutex_destroy(&clock
->clock_lock
);
874 put_device(&clock
->bus
->dev
);
878 mutex_unlock(&phyter_clocks_lock
);
881 static void dp83640_clock_init(struct dp83640_clock
*clock
, struct mii_bus
*bus
)
883 INIT_LIST_HEAD(&clock
->list
);
885 mutex_init(&clock
->extreg_lock
);
886 mutex_init(&clock
->clock_lock
);
887 INIT_LIST_HEAD(&clock
->phylist
);
888 clock
->caps
.owner
= THIS_MODULE
;
889 sprintf(clock
->caps
.name
, "dp83640 timer");
890 clock
->caps
.max_adj
= 1953124;
891 clock
->caps
.n_alarm
= 0;
892 clock
->caps
.n_ext_ts
= N_EXT_TS
;
893 clock
->caps
.n_per_out
= 1;
895 clock
->caps
.adjfreq
= ptp_dp83640_adjfreq
;
896 clock
->caps
.adjtime
= ptp_dp83640_adjtime
;
897 clock
->caps
.gettime
= ptp_dp83640_gettime
;
898 clock
->caps
.settime
= ptp_dp83640_settime
;
899 clock
->caps
.enable
= ptp_dp83640_enable
;
901 * Get a reference to this bus instance.
903 get_device(&bus
->dev
);
906 static int choose_this_phy(struct dp83640_clock
*clock
,
907 struct phy_device
*phydev
)
909 if (chosen_phy
== -1 && !clock
->chosen
)
912 if (chosen_phy
== phydev
->addr
)
918 static struct dp83640_clock
*dp83640_clock_get(struct dp83640_clock
*clock
)
921 mutex_lock(&clock
->clock_lock
);
926 * Look up and lock a clock by bus instance.
927 * If there is no clock for this bus, then create it first.
929 static struct dp83640_clock
*dp83640_clock_get_bus(struct mii_bus
*bus
)
931 struct dp83640_clock
*clock
= NULL
, *tmp
;
932 struct list_head
*this;
934 mutex_lock(&phyter_clocks_lock
);
936 list_for_each(this, &phyter_clocks
) {
937 tmp
= list_entry(this, struct dp83640_clock
, list
);
938 if (tmp
->bus
== bus
) {
946 clock
= kzalloc(sizeof(struct dp83640_clock
), GFP_KERNEL
);
950 dp83640_clock_init(clock
, bus
);
951 list_add_tail(&phyter_clocks
, &clock
->list
);
953 mutex_unlock(&phyter_clocks_lock
);
955 return dp83640_clock_get(clock
);
958 static void dp83640_clock_put(struct dp83640_clock
*clock
)
960 mutex_unlock(&clock
->clock_lock
);
963 static int dp83640_probe(struct phy_device
*phydev
)
965 struct dp83640_clock
*clock
;
966 struct dp83640_private
*dp83640
;
967 int err
= -ENOMEM
, i
;
969 if (phydev
->addr
== BROADCAST_ADDR
)
972 clock
= dp83640_clock_get_bus(phydev
->bus
);
976 dp83640
= kzalloc(sizeof(struct dp83640_private
), GFP_KERNEL
);
980 dp83640
->phydev
= phydev
;
981 INIT_WORK(&dp83640
->ts_work
, rx_timestamp_work
);
983 INIT_LIST_HEAD(&dp83640
->rxts
);
984 INIT_LIST_HEAD(&dp83640
->rxpool
);
985 for (i
= 0; i
< MAX_RXTS
; i
++)
986 list_add(&dp83640
->rx_pool_data
[i
].list
, &dp83640
->rxpool
);
988 phydev
->priv
= dp83640
;
990 spin_lock_init(&dp83640
->rx_lock
);
991 skb_queue_head_init(&dp83640
->rx_queue
);
992 skb_queue_head_init(&dp83640
->tx_queue
);
994 dp83640
->clock
= clock
;
996 if (choose_this_phy(clock
, phydev
)) {
997 clock
->chosen
= dp83640
;
998 clock
->ptp_clock
= ptp_clock_register(&clock
->caps
, &phydev
->dev
);
999 if (IS_ERR(clock
->ptp_clock
)) {
1000 err
= PTR_ERR(clock
->ptp_clock
);
1004 list_add_tail(&dp83640
->list
, &clock
->phylist
);
1006 if (clock
->chosen
&& !list_empty(&clock
->phylist
))
1009 enable_broadcast(dp83640
->phydev
, clock
->page
, 1);
1011 dp83640_clock_put(clock
);
1015 clock
->chosen
= NULL
;
1018 dp83640_clock_put(clock
);
1023 static void dp83640_remove(struct phy_device
*phydev
)
1025 struct dp83640_clock
*clock
;
1026 struct list_head
*this, *next
;
1027 struct dp83640_private
*tmp
, *dp83640
= phydev
->priv
;
1028 struct sk_buff
*skb
;
1030 if (phydev
->addr
== BROADCAST_ADDR
)
1033 enable_status_frames(phydev
, false);
1034 cancel_work_sync(&dp83640
->ts_work
);
1036 while ((skb
= skb_dequeue(&dp83640
->rx_queue
)) != NULL
)
1039 while ((skb
= skb_dequeue(&dp83640
->tx_queue
)) != NULL
)
1040 skb_complete_tx_timestamp(skb
, NULL
);
1042 clock
= dp83640_clock_get(dp83640
->clock
);
1044 if (dp83640
== clock
->chosen
) {
1045 ptp_clock_unregister(clock
->ptp_clock
);
1046 clock
->chosen
= NULL
;
1048 list_for_each_safe(this, next
, &clock
->phylist
) {
1049 tmp
= list_entry(this, struct dp83640_private
, list
);
1050 if (tmp
== dp83640
) {
1051 list_del_init(&tmp
->list
);
1057 dp83640_clock_put(clock
);
1061 static int dp83640_ack_interrupt(struct phy_device
*phydev
)
1063 int err
= phy_read(phydev
, MII_DP83640_MISR
);
1071 static int dp83640_config_intr(struct phy_device
*phydev
)
1077 if (phydev
->interrupts
== PHY_INTERRUPT_ENABLED
) {
1078 misr
= phy_read(phydev
, MII_DP83640_MISR
);
1082 (MII_DP83640_MISR_ANC_INT_EN
|
1083 MII_DP83640_MISR_DUP_INT_EN
|
1084 MII_DP83640_MISR_SPD_INT_EN
|
1085 MII_DP83640_MISR_LINK_INT_EN
);
1086 err
= phy_write(phydev
, MII_DP83640_MISR
, misr
);
1090 micr
= phy_read(phydev
, MII_DP83640_MICR
);
1094 (MII_DP83640_MICR_OE
|
1095 MII_DP83640_MICR_IE
);
1096 return phy_write(phydev
, MII_DP83640_MICR
, micr
);
1098 micr
= phy_read(phydev
, MII_DP83640_MICR
);
1102 ~(MII_DP83640_MICR_OE
|
1103 MII_DP83640_MICR_IE
);
1104 err
= phy_write(phydev
, MII_DP83640_MICR
, micr
);
1108 misr
= phy_read(phydev
, MII_DP83640_MISR
);
1112 ~(MII_DP83640_MISR_ANC_INT_EN
|
1113 MII_DP83640_MISR_DUP_INT_EN
|
1114 MII_DP83640_MISR_SPD_INT_EN
|
1115 MII_DP83640_MISR_LINK_INT_EN
);
1116 return phy_write(phydev
, MII_DP83640_MISR
, misr
);
1120 static int dp83640_hwtstamp(struct phy_device
*phydev
, struct ifreq
*ifr
)
1122 struct dp83640_private
*dp83640
= phydev
->priv
;
1123 struct hwtstamp_config cfg
;
1126 if (copy_from_user(&cfg
, ifr
->ifr_data
, sizeof(cfg
)))
1129 if (cfg
.flags
) /* reserved for future extensions */
1132 if (cfg
.tx_type
< 0 || cfg
.tx_type
> HWTSTAMP_TX_ONESTEP_SYNC
)
1135 dp83640
->hwts_tx_en
= cfg
.tx_type
;
1137 switch (cfg
.rx_filter
) {
1138 case HWTSTAMP_FILTER_NONE
:
1139 dp83640
->hwts_rx_en
= 0;
1141 dp83640
->version
= 0;
1143 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT
:
1144 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC
:
1145 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ
:
1146 dp83640
->hwts_rx_en
= 1;
1147 dp83640
->layer
= LAYER4
;
1148 dp83640
->version
= 1;
1150 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT
:
1151 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC
:
1152 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ
:
1153 dp83640
->hwts_rx_en
= 1;
1154 dp83640
->layer
= LAYER4
;
1155 dp83640
->version
= 2;
1157 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT
:
1158 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC
:
1159 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ
:
1160 dp83640
->hwts_rx_en
= 1;
1161 dp83640
->layer
= LAYER2
;
1162 dp83640
->version
= 2;
1164 case HWTSTAMP_FILTER_PTP_V2_EVENT
:
1165 case HWTSTAMP_FILTER_PTP_V2_SYNC
:
1166 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ
:
1167 dp83640
->hwts_rx_en
= 1;
1168 dp83640
->layer
= LAYER4
|LAYER2
;
1169 dp83640
->version
= 2;
1175 txcfg0
= (dp83640
->version
& TX_PTP_VER_MASK
) << TX_PTP_VER_SHIFT
;
1176 rxcfg0
= (dp83640
->version
& TX_PTP_VER_MASK
) << TX_PTP_VER_SHIFT
;
1178 if (dp83640
->layer
& LAYER2
) {
1182 if (dp83640
->layer
& LAYER4
) {
1183 txcfg0
|= TX_IPV6_EN
| TX_IPV4_EN
;
1184 rxcfg0
|= RX_IPV6_EN
| RX_IPV4_EN
;
1187 if (dp83640
->hwts_tx_en
)
1190 if (dp83640
->hwts_tx_en
== HWTSTAMP_TX_ONESTEP_SYNC
)
1191 txcfg0
|= SYNC_1STEP
| CHK_1STEP
;
1193 if (dp83640
->hwts_rx_en
)
1196 mutex_lock(&dp83640
->clock
->extreg_lock
);
1198 if (dp83640
->hwts_tx_en
|| dp83640
->hwts_rx_en
) {
1199 enable_status_frames(phydev
, true);
1200 ext_write(0, phydev
, PAGE4
, PTP_CTL
, PTP_ENABLE
);
1203 ext_write(0, phydev
, PAGE5
, PTP_TXCFG0
, txcfg0
);
1204 ext_write(0, phydev
, PAGE5
, PTP_RXCFG0
, rxcfg0
);
1206 mutex_unlock(&dp83640
->clock
->extreg_lock
);
1208 return copy_to_user(ifr
->ifr_data
, &cfg
, sizeof(cfg
)) ? -EFAULT
: 0;
1211 static void rx_timestamp_work(struct work_struct
*work
)
1213 struct dp83640_private
*dp83640
=
1214 container_of(work
, struct dp83640_private
, ts_work
);
1215 struct list_head
*this, *next
;
1217 struct skb_shared_hwtstamps
*shhwtstamps
;
1218 struct sk_buff
*skb
;
1220 unsigned long flags
;
1222 /* Deliver each deferred packet, with or without a time stamp. */
1224 while ((skb
= skb_dequeue(&dp83640
->rx_queue
)) != NULL
) {
1225 type
= SKB_PTP_TYPE(skb
);
1226 spin_lock_irqsave(&dp83640
->rx_lock
, flags
);
1227 list_for_each_safe(this, next
, &dp83640
->rxts
) {
1228 rxts
= list_entry(this, struct rxts
, list
);
1229 if (match(skb
, type
, rxts
)) {
1230 shhwtstamps
= skb_hwtstamps(skb
);
1231 memset(shhwtstamps
, 0, sizeof(*shhwtstamps
));
1232 shhwtstamps
->hwtstamp
= ns_to_ktime(rxts
->ns
);
1233 list_del_init(&rxts
->list
);
1234 list_add(&rxts
->list
, &dp83640
->rxpool
);
1238 spin_unlock_irqrestore(&dp83640
->rx_lock
, flags
);
1242 /* Clear out expired time stamps. */
1244 spin_lock_irqsave(&dp83640
->rx_lock
, flags
);
1245 prune_rx_ts(dp83640
);
1246 spin_unlock_irqrestore(&dp83640
->rx_lock
, flags
);
1249 static bool dp83640_rxtstamp(struct phy_device
*phydev
,
1250 struct sk_buff
*skb
, int type
)
1252 struct dp83640_private
*dp83640
= phydev
->priv
;
1254 if (!dp83640
->hwts_rx_en
)
1257 if (is_status_frame(skb
, type
)) {
1258 decode_status_frame(dp83640
, skb
);
1263 SKB_PTP_TYPE(skb
) = type
;
1264 skb_queue_tail(&dp83640
->rx_queue
, skb
);
1265 schedule_work(&dp83640
->ts_work
);
1270 static void dp83640_txtstamp(struct phy_device
*phydev
,
1271 struct sk_buff
*skb
, int type
)
1273 struct dp83640_private
*dp83640
= phydev
->priv
;
1275 switch (dp83640
->hwts_tx_en
) {
1277 case HWTSTAMP_TX_ONESTEP_SYNC
:
1278 if (is_sync(skb
, type
)) {
1279 skb_complete_tx_timestamp(skb
, NULL
);
1283 case HWTSTAMP_TX_ON
:
1284 skb_queue_tail(&dp83640
->tx_queue
, skb
);
1285 schedule_work(&dp83640
->ts_work
);
1288 case HWTSTAMP_TX_OFF
:
1290 skb_complete_tx_timestamp(skb
, NULL
);
1295 static int dp83640_ts_info(struct phy_device
*dev
, struct ethtool_ts_info
*info
)
1297 struct dp83640_private
*dp83640
= dev
->priv
;
1299 info
->so_timestamping
=
1300 SOF_TIMESTAMPING_TX_HARDWARE
|
1301 SOF_TIMESTAMPING_RX_HARDWARE
|
1302 SOF_TIMESTAMPING_RAW_HARDWARE
;
1303 info
->phc_index
= ptp_clock_index(dp83640
->clock
->ptp_clock
);
1305 (1 << HWTSTAMP_TX_OFF
) |
1306 (1 << HWTSTAMP_TX_ON
) |
1307 (1 << HWTSTAMP_TX_ONESTEP_SYNC
);
1309 (1 << HWTSTAMP_FILTER_NONE
) |
1310 (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT
) |
1311 (1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC
) |
1312 (1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ
) |
1313 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT
) |
1314 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC
) |
1315 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ
) |
1316 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT
) |
1317 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC
) |
1318 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ
) |
1319 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT
) |
1320 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC
) |
1321 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ
);
1325 static struct phy_driver dp83640_driver
= {
1326 .phy_id
= DP83640_PHY_ID
,
1327 .phy_id_mask
= 0xfffffff0,
1328 .name
= "NatSemi DP83640",
1329 .features
= PHY_BASIC_FEATURES
,
1330 .flags
= PHY_HAS_INTERRUPT
,
1331 .probe
= dp83640_probe
,
1332 .remove
= dp83640_remove
,
1333 .config_aneg
= genphy_config_aneg
,
1334 .read_status
= genphy_read_status
,
1335 .ack_interrupt
= dp83640_ack_interrupt
,
1336 .config_intr
= dp83640_config_intr
,
1337 .ts_info
= dp83640_ts_info
,
1338 .hwtstamp
= dp83640_hwtstamp
,
1339 .rxtstamp
= dp83640_rxtstamp
,
1340 .txtstamp
= dp83640_txtstamp
,
1341 .driver
= {.owner
= THIS_MODULE
,}
1344 static int __init
dp83640_init(void)
1346 return phy_driver_register(&dp83640_driver
);
1349 static void __exit
dp83640_exit(void)
1351 dp83640_free_clocks();
1352 phy_driver_unregister(&dp83640_driver
);
1355 MODULE_DESCRIPTION("National Semiconductor DP83640 PHY driver");
1356 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.at>");
1357 MODULE_LICENSE("GPL");
1359 module_init(dp83640_init
);
1360 module_exit(dp83640_exit
);
1362 static struct mdio_device_id __maybe_unused dp83640_tbl
[] = {
1363 { DP83640_PHY_ID
, 0xfffffff0 },
1367 MODULE_DEVICE_TABLE(mdio
, dp83640_tbl
);