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 /* phyter seems to miss the mark by 16 ns */
52 #define ADJTIME_FIX 16
54 #if defined(__BIG_ENDIAN)
56 #elif defined(__LITTLE_ENDIAN)
57 #define ENDIAN_FLAG PSF_ENDIAN
60 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
63 u16 ns_lo
; /* ns[15:0] */
64 u16 ns_hi
; /* overflow[1:0], ns[29:16] */
65 u16 sec_lo
; /* sec[15:0] */
66 u16 sec_hi
; /* sec[31:16] */
67 u16 seqid
; /* sequenceId[15:0] */
68 u16 msgtype
; /* messageType[3:0], hash[11:0] */
72 u16 ns_lo
; /* ns[15:0] */
73 u16 ns_hi
; /* overflow[1:0], ns[29:16] */
74 u16 sec_lo
; /* sec[15:0] */
75 u16 sec_hi
; /* sec[31:16] */
79 struct list_head list
;
89 struct dp83640_private
{
90 struct list_head list
;
91 struct dp83640_clock
*clock
;
92 struct phy_device
*phydev
;
93 struct work_struct ts_work
;
98 /* remember state of cfg0 during calibration */
100 /* remember the last event time stamp */
101 struct phy_txts edata
;
102 /* list of rx timestamps */
103 struct list_head rxts
;
104 struct list_head rxpool
;
105 struct rxts rx_pool_data
[MAX_RXTS
];
106 /* protects above three fields from concurrent access */
108 /* queues of incoming and outgoing packets */
109 struct sk_buff_head rx_queue
;
110 struct sk_buff_head tx_queue
;
113 struct dp83640_clock
{
114 /* keeps the instance in the 'phyter_clocks' list */
115 struct list_head list
;
116 /* we create one clock instance per MII bus */
118 /* protects extended registers from concurrent access */
119 struct mutex extreg_lock
;
120 /* remembers which page was last selected */
122 /* our advertised capabilities */
123 struct ptp_clock_info caps
;
124 /* protects the three fields below from concurrent access */
125 struct mutex clock_lock
;
126 /* the one phyter from which we shall read */
127 struct dp83640_private
*chosen
;
128 /* list of the other attached phyters, not chosen */
129 struct list_head phylist
;
130 /* reference to our PTP hardware clock */
131 struct ptp_clock
*ptp_clock
;
148 static int chosen_phy
= -1;
149 static ushort gpio_tab
[GPIO_TABLE_SIZE
] = {
150 1, 2, 3, 4, 8, 9, 10, 11
153 module_param(chosen_phy
, int, 0444);
154 module_param_array(gpio_tab
, ushort
, NULL
, 0444);
156 MODULE_PARM_DESC(chosen_phy
, \
157 "The address of the PHY to use for the ancillary clock features");
158 MODULE_PARM_DESC(gpio_tab
, \
159 "Which GPIO line to use for which purpose: cal,perout,extts1,...,extts6");
161 /* a list of clocks and a mutex to protect it */
162 static LIST_HEAD(phyter_clocks
);
163 static DEFINE_MUTEX(phyter_clocks_lock
);
165 static void rx_timestamp_work(struct work_struct
*work
);
167 /* extended register access functions */
169 #define BROADCAST_ADDR 31
171 static inline int broadcast_write(struct mii_bus
*bus
, u32 regnum
, u16 val
)
173 return mdiobus_write(bus
, BROADCAST_ADDR
, regnum
, val
);
176 /* Caller must hold extreg_lock. */
177 static int ext_read(struct phy_device
*phydev
, int page
, u32 regnum
)
179 struct dp83640_private
*dp83640
= phydev
->priv
;
182 if (dp83640
->clock
->page
!= page
) {
183 broadcast_write(phydev
->bus
, PAGESEL
, page
);
184 dp83640
->clock
->page
= page
;
186 val
= phy_read(phydev
, regnum
);
191 /* Caller must hold extreg_lock. */
192 static void ext_write(int broadcast
, struct phy_device
*phydev
,
193 int page
, u32 regnum
, u16 val
)
195 struct dp83640_private
*dp83640
= phydev
->priv
;
197 if (dp83640
->clock
->page
!= page
) {
198 broadcast_write(phydev
->bus
, PAGESEL
, page
);
199 dp83640
->clock
->page
= page
;
202 broadcast_write(phydev
->bus
, regnum
, val
);
204 phy_write(phydev
, regnum
, val
);
207 /* Caller must hold extreg_lock. */
208 static int tdr_write(int bc
, struct phy_device
*dev
,
209 const struct timespec
*ts
, u16 cmd
)
211 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_nsec
& 0xffff);/* ns[15:0] */
212 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_nsec
>> 16); /* ns[31:16] */
213 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_sec
& 0xffff); /* sec[15:0] */
214 ext_write(bc
, dev
, PAGE4
, PTP_TDR
, ts
->tv_sec
>> 16); /* sec[31:16]*/
216 ext_write(bc
, dev
, PAGE4
, PTP_CTL
, cmd
);
221 /* convert phy timestamps into driver timestamps */
223 static void phy2rxts(struct phy_rxts
*p
, struct rxts
*rxts
)
228 sec
|= p
->sec_hi
<< 16;
231 rxts
->ns
|= (p
->ns_hi
& 0x3fff) << 16;
232 rxts
->ns
+= ((u64
)sec
) * 1000000000ULL;
233 rxts
->seqid
= p
->seqid
;
234 rxts
->msgtype
= (p
->msgtype
>> 12) & 0xf;
235 rxts
->hash
= p
->msgtype
& 0x0fff;
236 rxts
->tmo
= jiffies
+ 2;
239 static u64
phy2txts(struct phy_txts
*p
)
245 sec
|= p
->sec_hi
<< 16;
248 ns
|= (p
->ns_hi
& 0x3fff) << 16;
249 ns
+= ((u64
)sec
) * 1000000000ULL;
254 static void periodic_output(struct dp83640_clock
*clock
,
255 struct ptp_clock_request
*clkreq
, bool on
)
257 struct dp83640_private
*dp83640
= clock
->chosen
;
258 struct phy_device
*phydev
= dp83640
->phydev
;
259 u32 sec
, nsec
, period
;
260 u16 gpio
, ptp_trig
, trigger
, val
;
262 gpio
= on
? gpio_tab
[PEROUT_GPIO
] : 0;
263 trigger
= PER_TRIGGER
;
266 (trigger
& TRIG_CSEL_MASK
) << TRIG_CSEL_SHIFT
|
267 (gpio
& TRIG_GPIO_MASK
) << TRIG_GPIO_SHIFT
|
271 val
= (trigger
& TRIG_SEL_MASK
) << TRIG_SEL_SHIFT
;
275 mutex_lock(&clock
->extreg_lock
);
276 ext_write(0, phydev
, PAGE5
, PTP_TRIG
, ptp_trig
);
277 ext_write(0, phydev
, PAGE4
, PTP_CTL
, val
);
278 mutex_unlock(&clock
->extreg_lock
);
282 sec
= clkreq
->perout
.start
.sec
;
283 nsec
= clkreq
->perout
.start
.nsec
;
284 period
= clkreq
->perout
.period
.sec
* 1000000000UL;
285 period
+= clkreq
->perout
.period
.nsec
;
287 mutex_lock(&clock
->extreg_lock
);
289 ext_write(0, phydev
, PAGE5
, PTP_TRIG
, ptp_trig
);
293 ext_write(0, phydev
, PAGE4
, PTP_CTL
, val
);
294 ext_write(0, phydev
, PAGE4
, PTP_TDR
, nsec
& 0xffff); /* ns[15:0] */
295 ext_write(0, phydev
, PAGE4
, PTP_TDR
, nsec
>> 16); /* ns[31:16] */
296 ext_write(0, phydev
, PAGE4
, PTP_TDR
, sec
& 0xffff); /* sec[15:0] */
297 ext_write(0, phydev
, PAGE4
, PTP_TDR
, sec
>> 16); /* sec[31:16] */
298 ext_write(0, phydev
, PAGE4
, PTP_TDR
, period
& 0xffff); /* ns[15:0] */
299 ext_write(0, phydev
, PAGE4
, PTP_TDR
, period
>> 16); /* ns[31:16] */
304 ext_write(0, phydev
, PAGE4
, PTP_CTL
, val
);
306 mutex_unlock(&clock
->extreg_lock
);
309 /* ptp clock methods */
311 static int ptp_dp83640_adjfreq(struct ptp_clock_info
*ptp
, s32 ppb
)
313 struct dp83640_clock
*clock
=
314 container_of(ptp
, struct dp83640_clock
, caps
);
315 struct phy_device
*phydev
= clock
->chosen
->phydev
;
326 rate
= div_u64(rate
, 1953125);
328 hi
= (rate
>> 16) & PTP_RATE_HI_MASK
;
334 mutex_lock(&clock
->extreg_lock
);
336 ext_write(1, phydev
, PAGE4
, PTP_RATEH
, hi
);
337 ext_write(1, phydev
, PAGE4
, PTP_RATEL
, lo
);
339 mutex_unlock(&clock
->extreg_lock
);
344 static int ptp_dp83640_adjtime(struct ptp_clock_info
*ptp
, s64 delta
)
346 struct dp83640_clock
*clock
=
347 container_of(ptp
, struct dp83640_clock
, caps
);
348 struct phy_device
*phydev
= clock
->chosen
->phydev
;
352 delta
+= ADJTIME_FIX
;
354 ts
= ns_to_timespec(delta
);
356 mutex_lock(&clock
->extreg_lock
);
358 err
= tdr_write(1, phydev
, &ts
, PTP_STEP_CLK
);
360 mutex_unlock(&clock
->extreg_lock
);
365 static int ptp_dp83640_gettime(struct ptp_clock_info
*ptp
, struct timespec
*ts
)
367 struct dp83640_clock
*clock
=
368 container_of(ptp
, struct dp83640_clock
, caps
);
369 struct phy_device
*phydev
= clock
->chosen
->phydev
;
372 mutex_lock(&clock
->extreg_lock
);
374 ext_write(0, phydev
, PAGE4
, PTP_CTL
, PTP_RD_CLK
);
376 val
[0] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* ns[15:0] */
377 val
[1] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* ns[31:16] */
378 val
[2] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* sec[15:0] */
379 val
[3] = ext_read(phydev
, PAGE4
, PTP_TDR
); /* sec[31:16] */
381 mutex_unlock(&clock
->extreg_lock
);
383 ts
->tv_nsec
= val
[0] | (val
[1] << 16);
384 ts
->tv_sec
= val
[2] | (val
[3] << 16);
389 static int ptp_dp83640_settime(struct ptp_clock_info
*ptp
,
390 const struct timespec
*ts
)
392 struct dp83640_clock
*clock
=
393 container_of(ptp
, struct dp83640_clock
, caps
);
394 struct phy_device
*phydev
= clock
->chosen
->phydev
;
397 mutex_lock(&clock
->extreg_lock
);
399 err
= tdr_write(1, phydev
, ts
, PTP_LOAD_CLK
);
401 mutex_unlock(&clock
->extreg_lock
);
406 static int ptp_dp83640_enable(struct ptp_clock_info
*ptp
,
407 struct ptp_clock_request
*rq
, int on
)
409 struct dp83640_clock
*clock
=
410 container_of(ptp
, struct dp83640_clock
, caps
);
411 struct phy_device
*phydev
= clock
->chosen
->phydev
;
413 u16 evnt
, event_num
, gpio_num
;
416 case PTP_CLK_REQ_EXTTS
:
417 index
= rq
->extts
.index
;
418 if (index
< 0 || index
>= N_EXT_TS
)
420 event_num
= EXT_EVENT
+ index
;
421 evnt
= EVNT_WR
| (event_num
& EVNT_SEL_MASK
) << EVNT_SEL_SHIFT
;
423 gpio_num
= gpio_tab
[EXTTS0_GPIO
+ index
];
424 evnt
|= (gpio_num
& EVNT_GPIO_MASK
) << EVNT_GPIO_SHIFT
;
427 ext_write(0, phydev
, PAGE5
, PTP_EVNT
, evnt
);
430 case PTP_CLK_REQ_PEROUT
:
431 if (rq
->perout
.index
!= 0)
433 periodic_output(clock
, rq
, on
);
443 static u8 status_frame_dst
[6] = { 0x01, 0x1B, 0x19, 0x00, 0x00, 0x00 };
444 static u8 status_frame_src
[6] = { 0x08, 0x00, 0x17, 0x0B, 0x6B, 0x0F };
446 static void enable_status_frames(struct phy_device
*phydev
, bool on
)
451 cfg0
= PSF_EVNT_EN
| PSF_RXTS_EN
| PSF_TXTS_EN
| ENDIAN_FLAG
;
453 ver
= (PSF_PTPVER
& VERSIONPTP_MASK
) << VERSIONPTP_SHIFT
;
455 ext_write(0, phydev
, PAGE5
, PSF_CFG0
, cfg0
);
456 ext_write(0, phydev
, PAGE6
, PSF_CFG1
, ver
);
458 if (!phydev
->attached_dev
) {
459 pr_warn("expected to find an attached netdevice\n");
464 if (dev_mc_add(phydev
->attached_dev
, status_frame_dst
))
465 pr_warn("failed to add mc address\n");
467 if (dev_mc_del(phydev
->attached_dev
, status_frame_dst
))
468 pr_warn("failed to delete mc address\n");
472 static bool is_status_frame(struct sk_buff
*skb
, int type
)
474 struct ethhdr
*h
= eth_hdr(skb
);
476 if (PTP_CLASS_V2_L2
== type
&&
477 !memcmp(h
->h_source
, status_frame_src
, sizeof(status_frame_src
)))
483 static int expired(struct rxts
*rxts
)
485 return time_after(jiffies
, rxts
->tmo
);
488 /* Caller must hold rx_lock. */
489 static void prune_rx_ts(struct dp83640_private
*dp83640
)
491 struct list_head
*this, *next
;
494 list_for_each_safe(this, next
, &dp83640
->rxts
) {
495 rxts
= list_entry(this, struct rxts
, list
);
497 list_del_init(&rxts
->list
);
498 list_add(&rxts
->list
, &dp83640
->rxpool
);
503 /* synchronize the phyters so they act as one clock */
505 static void enable_broadcast(struct phy_device
*phydev
, int init_page
, int on
)
508 phy_write(phydev
, PAGESEL
, 0);
509 val
= phy_read(phydev
, PHYCR2
);
514 phy_write(phydev
, PHYCR2
, val
);
515 phy_write(phydev
, PAGESEL
, init_page
);
518 static void recalibrate(struct dp83640_clock
*clock
)
521 struct phy_txts event_ts
;
523 struct list_head
*this;
524 struct dp83640_private
*tmp
;
525 struct phy_device
*master
= clock
->chosen
->phydev
;
526 u16 cal_gpio
, cfg0
, evnt
, ptp_trig
, trigger
, val
;
528 trigger
= CAL_TRIGGER
;
529 cal_gpio
= gpio_tab
[CALIBRATE_GPIO
];
531 mutex_lock(&clock
->extreg_lock
);
534 * enable broadcast, disable status frames, enable ptp clock
536 list_for_each(this, &clock
->phylist
) {
537 tmp
= list_entry(this, struct dp83640_private
, list
);
538 enable_broadcast(tmp
->phydev
, clock
->page
, 1);
539 tmp
->cfg0
= ext_read(tmp
->phydev
, PAGE5
, PSF_CFG0
);
540 ext_write(0, tmp
->phydev
, PAGE5
, PSF_CFG0
, 0);
541 ext_write(0, tmp
->phydev
, PAGE4
, PTP_CTL
, PTP_ENABLE
);
543 enable_broadcast(master
, clock
->page
, 1);
544 cfg0
= ext_read(master
, PAGE5
, PSF_CFG0
);
545 ext_write(0, master
, PAGE5
, PSF_CFG0
, 0);
546 ext_write(0, master
, PAGE4
, PTP_CTL
, PTP_ENABLE
);
549 * enable an event timestamp
551 evnt
= EVNT_WR
| EVNT_RISE
| EVNT_SINGLE
;
552 evnt
|= (CAL_EVENT
& EVNT_SEL_MASK
) << EVNT_SEL_SHIFT
;
553 evnt
|= (cal_gpio
& EVNT_GPIO_MASK
) << EVNT_GPIO_SHIFT
;
555 list_for_each(this, &clock
->phylist
) {
556 tmp
= list_entry(this, struct dp83640_private
, list
);
557 ext_write(0, tmp
->phydev
, PAGE5
, PTP_EVNT
, evnt
);
559 ext_write(0, master
, PAGE5
, PTP_EVNT
, evnt
);
562 * configure a trigger
564 ptp_trig
= TRIG_WR
| TRIG_IF_LATE
| TRIG_PULSE
;
565 ptp_trig
|= (trigger
& TRIG_CSEL_MASK
) << TRIG_CSEL_SHIFT
;
566 ptp_trig
|= (cal_gpio
& TRIG_GPIO_MASK
) << TRIG_GPIO_SHIFT
;
567 ext_write(0, master
, PAGE5
, PTP_TRIG
, ptp_trig
);
570 val
= (trigger
& TRIG_SEL_MASK
) << TRIG_SEL_SHIFT
;
572 ext_write(0, master
, PAGE4
, PTP_CTL
, val
);
577 ext_write(0, master
, PAGE4
, PTP_CTL
, val
);
579 /* disable trigger */
580 val
= (trigger
& TRIG_SEL_MASK
) << TRIG_SEL_SHIFT
;
582 ext_write(0, master
, PAGE4
, PTP_CTL
, val
);
585 * read out and correct offsets
587 val
= ext_read(master
, PAGE4
, PTP_STS
);
588 pr_info("master PTP_STS 0x%04hx\n", val
);
589 val
= ext_read(master
, PAGE4
, PTP_ESTS
);
590 pr_info("master PTP_ESTS 0x%04hx\n", val
);
591 event_ts
.ns_lo
= ext_read(master
, PAGE4
, PTP_EDATA
);
592 event_ts
.ns_hi
= ext_read(master
, PAGE4
, PTP_EDATA
);
593 event_ts
.sec_lo
= ext_read(master
, PAGE4
, PTP_EDATA
);
594 event_ts
.sec_hi
= ext_read(master
, PAGE4
, PTP_EDATA
);
595 now
= phy2txts(&event_ts
);
597 list_for_each(this, &clock
->phylist
) {
598 tmp
= list_entry(this, struct dp83640_private
, list
);
599 val
= ext_read(tmp
->phydev
, PAGE4
, PTP_STS
);
600 pr_info("slave PTP_STS 0x%04hx\n", val
);
601 val
= ext_read(tmp
->phydev
, PAGE4
, PTP_ESTS
);
602 pr_info("slave PTP_ESTS 0x%04hx\n", val
);
603 event_ts
.ns_lo
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
604 event_ts
.ns_hi
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
605 event_ts
.sec_lo
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
606 event_ts
.sec_hi
= ext_read(tmp
->phydev
, PAGE4
, PTP_EDATA
);
607 diff
= now
- (s64
) phy2txts(&event_ts
);
608 pr_info("slave offset %lld nanoseconds\n", diff
);
610 ts
= ns_to_timespec(diff
);
611 tdr_write(0, tmp
->phydev
, &ts
, PTP_STEP_CLK
);
615 * restore status frames
617 list_for_each(this, &clock
->phylist
) {
618 tmp
= list_entry(this, struct dp83640_private
, list
);
619 ext_write(0, tmp
->phydev
, PAGE5
, PSF_CFG0
, tmp
->cfg0
);
621 ext_write(0, master
, PAGE5
, PSF_CFG0
, cfg0
);
623 mutex_unlock(&clock
->extreg_lock
);
626 /* time stamping methods */
628 static inline u16
exts_chan_to_edata(int ch
)
630 return 1 << ((ch
+ EXT_EVENT
) * 2);
633 static int decode_evnt(struct dp83640_private
*dp83640
,
634 void *data
, u16 ests
)
636 struct phy_txts
*phy_txts
;
637 struct ptp_clock_event event
;
639 int words
= (ests
>> EVNT_TS_LEN_SHIFT
) & EVNT_TS_LEN_MASK
;
642 if (ests
& MULT_EVNT
) {
643 ext_status
= *(u16
*) data
;
644 data
+= sizeof(ext_status
);
649 switch (words
) { /* fall through in every case */
651 dp83640
->edata
.sec_hi
= phy_txts
->sec_hi
;
653 dp83640
->edata
.sec_lo
= phy_txts
->sec_lo
;
655 dp83640
->edata
.ns_hi
= phy_txts
->ns_hi
;
657 dp83640
->edata
.ns_lo
= phy_txts
->ns_lo
;
664 i
= ((ests
>> EVNT_NUM_SHIFT
) & EVNT_NUM_MASK
) - EXT_EVENT
;
665 ext_status
= exts_chan_to_edata(i
);
668 event
.type
= PTP_CLOCK_EXTTS
;
669 event
.timestamp
= phy2txts(&dp83640
->edata
);
671 for (i
= 0; i
< N_EXT_TS
; i
++) {
672 if (ext_status
& exts_chan_to_edata(i
)) {
674 ptp_clock_event(dp83640
->clock
->ptp_clock
, &event
);
678 return parsed
* sizeof(u16
);
681 static void decode_rxts(struct dp83640_private
*dp83640
,
682 struct phy_rxts
*phy_rxts
)
687 spin_lock_irqsave(&dp83640
->rx_lock
, flags
);
689 prune_rx_ts(dp83640
);
691 if (list_empty(&dp83640
->rxpool
)) {
692 pr_debug("rx timestamp pool is empty\n");
695 rxts
= list_first_entry(&dp83640
->rxpool
, struct rxts
, list
);
696 list_del_init(&rxts
->list
);
697 phy2rxts(phy_rxts
, rxts
);
698 list_add_tail(&rxts
->list
, &dp83640
->rxts
);
700 spin_unlock_irqrestore(&dp83640
->rx_lock
, flags
);
703 static void decode_txts(struct dp83640_private
*dp83640
,
704 struct phy_txts
*phy_txts
)
706 struct skb_shared_hwtstamps shhwtstamps
;
710 /* We must already have the skb that triggered this. */
712 skb
= skb_dequeue(&dp83640
->tx_queue
);
715 pr_debug("have timestamp but tx_queue empty\n");
718 ns
= phy2txts(phy_txts
);
719 memset(&shhwtstamps
, 0, sizeof(shhwtstamps
));
720 shhwtstamps
.hwtstamp
= ns_to_ktime(ns
);
721 skb_complete_tx_timestamp(skb
, &shhwtstamps
);
724 static void decode_status_frame(struct dp83640_private
*dp83640
,
727 struct phy_rxts
*phy_rxts
;
728 struct phy_txts
*phy_txts
;
735 for (len
= skb_headlen(skb
) - 2; len
> sizeof(type
); len
-= size
) {
738 ests
= type
& 0x0fff;
739 type
= type
& 0xf000;
743 if (PSF_RX
== type
&& len
>= sizeof(*phy_rxts
)) {
745 phy_rxts
= (struct phy_rxts
*) ptr
;
746 decode_rxts(dp83640
, phy_rxts
);
747 size
= sizeof(*phy_rxts
);
749 } else if (PSF_TX
== type
&& len
>= sizeof(*phy_txts
)) {
751 phy_txts
= (struct phy_txts
*) ptr
;
752 decode_txts(dp83640
, phy_txts
);
753 size
= sizeof(*phy_txts
);
755 } else if (PSF_EVNT
== type
&& len
>= sizeof(*phy_txts
)) {
757 size
= decode_evnt(dp83640
, ptr
, ests
);
767 static int is_sync(struct sk_buff
*skb
, int type
)
769 u8
*data
= skb
->data
, *msgtype
;
770 unsigned int offset
= 0;
773 case PTP_CLASS_V1_IPV4
:
774 case PTP_CLASS_V2_IPV4
:
775 offset
= ETH_HLEN
+ IPV4_HLEN(data
) + UDP_HLEN
;
777 case PTP_CLASS_V1_IPV6
:
778 case PTP_CLASS_V2_IPV6
:
781 case PTP_CLASS_V2_L2
:
784 case PTP_CLASS_V2_VLAN
:
785 offset
= ETH_HLEN
+ VLAN_HLEN
;
791 if (type
& PTP_CLASS_V1
)
792 offset
+= OFF_PTP_CONTROL
;
794 if (skb
->len
< offset
+ 1)
797 msgtype
= data
+ offset
;
799 return (*msgtype
& 0xf) == 0;
802 static int match(struct sk_buff
*skb
, unsigned int type
, struct rxts
*rxts
)
806 u8
*msgtype
, *data
= skb_mac_header(skb
);
808 /* check sequenceID, messageType, 12 bit hash of offset 20-29 */
811 case PTP_CLASS_V1_IPV4
:
812 case PTP_CLASS_V2_IPV4
:
813 offset
= ETH_HLEN
+ IPV4_HLEN(data
) + UDP_HLEN
;
815 case PTP_CLASS_V1_IPV6
:
816 case PTP_CLASS_V2_IPV6
:
819 case PTP_CLASS_V2_L2
:
822 case PTP_CLASS_V2_VLAN
:
823 offset
= ETH_HLEN
+ VLAN_HLEN
;
829 if (skb
->len
+ ETH_HLEN
< offset
+ OFF_PTP_SEQUENCE_ID
+ sizeof(*seqid
))
832 if (unlikely(type
& PTP_CLASS_V1
))
833 msgtype
= data
+ offset
+ OFF_PTP_CONTROL
;
835 msgtype
= data
+ offset
;
837 seqid
= (u16
*)(data
+ offset
+ OFF_PTP_SEQUENCE_ID
);
839 return (rxts
->msgtype
== (*msgtype
& 0xf) &&
840 rxts
->seqid
== ntohs(*seqid
));
843 static void dp83640_free_clocks(void)
845 struct dp83640_clock
*clock
;
846 struct list_head
*this, *next
;
848 mutex_lock(&phyter_clocks_lock
);
850 list_for_each_safe(this, next
, &phyter_clocks
) {
851 clock
= list_entry(this, struct dp83640_clock
, list
);
852 if (!list_empty(&clock
->phylist
)) {
853 pr_warn("phy list non-empty while unloading\n");
856 list_del(&clock
->list
);
857 mutex_destroy(&clock
->extreg_lock
);
858 mutex_destroy(&clock
->clock_lock
);
859 put_device(&clock
->bus
->dev
);
863 mutex_unlock(&phyter_clocks_lock
);
866 static void dp83640_clock_init(struct dp83640_clock
*clock
, struct mii_bus
*bus
)
868 INIT_LIST_HEAD(&clock
->list
);
870 mutex_init(&clock
->extreg_lock
);
871 mutex_init(&clock
->clock_lock
);
872 INIT_LIST_HEAD(&clock
->phylist
);
873 clock
->caps
.owner
= THIS_MODULE
;
874 sprintf(clock
->caps
.name
, "dp83640 timer");
875 clock
->caps
.max_adj
= 1953124;
876 clock
->caps
.n_alarm
= 0;
877 clock
->caps
.n_ext_ts
= N_EXT_TS
;
878 clock
->caps
.n_per_out
= 1;
880 clock
->caps
.adjfreq
= ptp_dp83640_adjfreq
;
881 clock
->caps
.adjtime
= ptp_dp83640_adjtime
;
882 clock
->caps
.gettime
= ptp_dp83640_gettime
;
883 clock
->caps
.settime
= ptp_dp83640_settime
;
884 clock
->caps
.enable
= ptp_dp83640_enable
;
886 * Get a reference to this bus instance.
888 get_device(&bus
->dev
);
891 static int choose_this_phy(struct dp83640_clock
*clock
,
892 struct phy_device
*phydev
)
894 if (chosen_phy
== -1 && !clock
->chosen
)
897 if (chosen_phy
== phydev
->addr
)
903 static struct dp83640_clock
*dp83640_clock_get(struct dp83640_clock
*clock
)
906 mutex_lock(&clock
->clock_lock
);
911 * Look up and lock a clock by bus instance.
912 * If there is no clock for this bus, then create it first.
914 static struct dp83640_clock
*dp83640_clock_get_bus(struct mii_bus
*bus
)
916 struct dp83640_clock
*clock
= NULL
, *tmp
;
917 struct list_head
*this;
919 mutex_lock(&phyter_clocks_lock
);
921 list_for_each(this, &phyter_clocks
) {
922 tmp
= list_entry(this, struct dp83640_clock
, list
);
923 if (tmp
->bus
== bus
) {
931 clock
= kzalloc(sizeof(struct dp83640_clock
), GFP_KERNEL
);
935 dp83640_clock_init(clock
, bus
);
936 list_add_tail(&phyter_clocks
, &clock
->list
);
938 mutex_unlock(&phyter_clocks_lock
);
940 return dp83640_clock_get(clock
);
943 static void dp83640_clock_put(struct dp83640_clock
*clock
)
945 mutex_unlock(&clock
->clock_lock
);
948 static int dp83640_probe(struct phy_device
*phydev
)
950 struct dp83640_clock
*clock
;
951 struct dp83640_private
*dp83640
;
952 int err
= -ENOMEM
, i
;
954 if (phydev
->addr
== BROADCAST_ADDR
)
957 clock
= dp83640_clock_get_bus(phydev
->bus
);
961 dp83640
= kzalloc(sizeof(struct dp83640_private
), GFP_KERNEL
);
965 dp83640
->phydev
= phydev
;
966 INIT_WORK(&dp83640
->ts_work
, rx_timestamp_work
);
968 INIT_LIST_HEAD(&dp83640
->rxts
);
969 INIT_LIST_HEAD(&dp83640
->rxpool
);
970 for (i
= 0; i
< MAX_RXTS
; i
++)
971 list_add(&dp83640
->rx_pool_data
[i
].list
, &dp83640
->rxpool
);
973 phydev
->priv
= dp83640
;
975 spin_lock_init(&dp83640
->rx_lock
);
976 skb_queue_head_init(&dp83640
->rx_queue
);
977 skb_queue_head_init(&dp83640
->tx_queue
);
979 dp83640
->clock
= clock
;
981 if (choose_this_phy(clock
, phydev
)) {
982 clock
->chosen
= dp83640
;
983 clock
->ptp_clock
= ptp_clock_register(&clock
->caps
);
984 if (IS_ERR(clock
->ptp_clock
)) {
985 err
= PTR_ERR(clock
->ptp_clock
);
989 list_add_tail(&dp83640
->list
, &clock
->phylist
);
991 if (clock
->chosen
&& !list_empty(&clock
->phylist
))
994 enable_broadcast(dp83640
->phydev
, clock
->page
, 1);
996 dp83640_clock_put(clock
);
1000 clock
->chosen
= NULL
;
1003 dp83640_clock_put(clock
);
1008 static void dp83640_remove(struct phy_device
*phydev
)
1010 struct dp83640_clock
*clock
;
1011 struct list_head
*this, *next
;
1012 struct dp83640_private
*tmp
, *dp83640
= phydev
->priv
;
1013 struct sk_buff
*skb
;
1015 if (phydev
->addr
== BROADCAST_ADDR
)
1018 enable_status_frames(phydev
, false);
1019 cancel_work_sync(&dp83640
->ts_work
);
1021 while ((skb
= skb_dequeue(&dp83640
->rx_queue
)) != NULL
)
1024 while ((skb
= skb_dequeue(&dp83640
->tx_queue
)) != NULL
)
1025 skb_complete_tx_timestamp(skb
, NULL
);
1027 clock
= dp83640_clock_get(dp83640
->clock
);
1029 if (dp83640
== clock
->chosen
) {
1030 ptp_clock_unregister(clock
->ptp_clock
);
1031 clock
->chosen
= NULL
;
1033 list_for_each_safe(this, next
, &clock
->phylist
) {
1034 tmp
= list_entry(this, struct dp83640_private
, list
);
1035 if (tmp
== dp83640
) {
1036 list_del_init(&tmp
->list
);
1042 dp83640_clock_put(clock
);
1046 static int dp83640_hwtstamp(struct phy_device
*phydev
, struct ifreq
*ifr
)
1048 struct dp83640_private
*dp83640
= phydev
->priv
;
1049 struct hwtstamp_config cfg
;
1052 if (copy_from_user(&cfg
, ifr
->ifr_data
, sizeof(cfg
)))
1055 if (cfg
.flags
) /* reserved for future extensions */
1058 if (cfg
.tx_type
< 0 || cfg
.tx_type
> HWTSTAMP_TX_ONESTEP_SYNC
)
1061 dp83640
->hwts_tx_en
= cfg
.tx_type
;
1063 switch (cfg
.rx_filter
) {
1064 case HWTSTAMP_FILTER_NONE
:
1065 dp83640
->hwts_rx_en
= 0;
1067 dp83640
->version
= 0;
1069 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT
:
1070 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC
:
1071 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ
:
1072 dp83640
->hwts_rx_en
= 1;
1073 dp83640
->layer
= LAYER4
;
1074 dp83640
->version
= 1;
1076 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT
:
1077 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC
:
1078 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ
:
1079 dp83640
->hwts_rx_en
= 1;
1080 dp83640
->layer
= LAYER4
;
1081 dp83640
->version
= 2;
1083 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT
:
1084 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC
:
1085 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ
:
1086 dp83640
->hwts_rx_en
= 1;
1087 dp83640
->layer
= LAYER2
;
1088 dp83640
->version
= 2;
1090 case HWTSTAMP_FILTER_PTP_V2_EVENT
:
1091 case HWTSTAMP_FILTER_PTP_V2_SYNC
:
1092 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ
:
1093 dp83640
->hwts_rx_en
= 1;
1094 dp83640
->layer
= LAYER4
|LAYER2
;
1095 dp83640
->version
= 2;
1101 txcfg0
= (dp83640
->version
& TX_PTP_VER_MASK
) << TX_PTP_VER_SHIFT
;
1102 rxcfg0
= (dp83640
->version
& TX_PTP_VER_MASK
) << TX_PTP_VER_SHIFT
;
1104 if (dp83640
->layer
& LAYER2
) {
1108 if (dp83640
->layer
& LAYER4
) {
1109 txcfg0
|= TX_IPV6_EN
| TX_IPV4_EN
;
1110 rxcfg0
|= RX_IPV6_EN
| RX_IPV4_EN
;
1113 if (dp83640
->hwts_tx_en
)
1116 if (dp83640
->hwts_tx_en
== HWTSTAMP_TX_ONESTEP_SYNC
)
1117 txcfg0
|= SYNC_1STEP
| CHK_1STEP
;
1119 if (dp83640
->hwts_rx_en
)
1122 mutex_lock(&dp83640
->clock
->extreg_lock
);
1124 if (dp83640
->hwts_tx_en
|| dp83640
->hwts_rx_en
) {
1125 enable_status_frames(phydev
, true);
1126 ext_write(0, phydev
, PAGE4
, PTP_CTL
, PTP_ENABLE
);
1129 ext_write(0, phydev
, PAGE5
, PTP_TXCFG0
, txcfg0
);
1130 ext_write(0, phydev
, PAGE5
, PTP_RXCFG0
, rxcfg0
);
1132 mutex_unlock(&dp83640
->clock
->extreg_lock
);
1134 return copy_to_user(ifr
->ifr_data
, &cfg
, sizeof(cfg
)) ? -EFAULT
: 0;
1137 static void rx_timestamp_work(struct work_struct
*work
)
1139 struct dp83640_private
*dp83640
=
1140 container_of(work
, struct dp83640_private
, ts_work
);
1141 struct list_head
*this, *next
;
1143 struct skb_shared_hwtstamps
*shhwtstamps
;
1144 struct sk_buff
*skb
;
1146 unsigned long flags
;
1148 /* Deliver each deferred packet, with or without a time stamp. */
1150 while ((skb
= skb_dequeue(&dp83640
->rx_queue
)) != NULL
) {
1151 type
= SKB_PTP_TYPE(skb
);
1152 spin_lock_irqsave(&dp83640
->rx_lock
, flags
);
1153 list_for_each_safe(this, next
, &dp83640
->rxts
) {
1154 rxts
= list_entry(this, struct rxts
, list
);
1155 if (match(skb
, type
, rxts
)) {
1156 shhwtstamps
= skb_hwtstamps(skb
);
1157 memset(shhwtstamps
, 0, sizeof(*shhwtstamps
));
1158 shhwtstamps
->hwtstamp
= ns_to_ktime(rxts
->ns
);
1159 list_del_init(&rxts
->list
);
1160 list_add(&rxts
->list
, &dp83640
->rxpool
);
1164 spin_unlock_irqrestore(&dp83640
->rx_lock
, flags
);
1168 /* Clear out expired time stamps. */
1170 spin_lock_irqsave(&dp83640
->rx_lock
, flags
);
1171 prune_rx_ts(dp83640
);
1172 spin_unlock_irqrestore(&dp83640
->rx_lock
, flags
);
1175 static bool dp83640_rxtstamp(struct phy_device
*phydev
,
1176 struct sk_buff
*skb
, int type
)
1178 struct dp83640_private
*dp83640
= phydev
->priv
;
1180 if (!dp83640
->hwts_rx_en
)
1183 if (is_status_frame(skb
, type
)) {
1184 decode_status_frame(dp83640
, skb
);
1189 SKB_PTP_TYPE(skb
) = type
;
1190 skb_queue_tail(&dp83640
->rx_queue
, skb
);
1191 schedule_work(&dp83640
->ts_work
);
1196 static void dp83640_txtstamp(struct phy_device
*phydev
,
1197 struct sk_buff
*skb
, int type
)
1199 struct dp83640_private
*dp83640
= phydev
->priv
;
1201 switch (dp83640
->hwts_tx_en
) {
1203 case HWTSTAMP_TX_ONESTEP_SYNC
:
1204 if (is_sync(skb
, type
)) {
1205 skb_complete_tx_timestamp(skb
, NULL
);
1209 case HWTSTAMP_TX_ON
:
1210 skb_queue_tail(&dp83640
->tx_queue
, skb
);
1211 schedule_work(&dp83640
->ts_work
);
1214 case HWTSTAMP_TX_OFF
:
1216 skb_complete_tx_timestamp(skb
, NULL
);
1221 static int dp83640_ts_info(struct phy_device
*dev
, struct ethtool_ts_info
*info
)
1223 struct dp83640_private
*dp83640
= dev
->priv
;
1225 info
->so_timestamping
=
1226 SOF_TIMESTAMPING_TX_HARDWARE
|
1227 SOF_TIMESTAMPING_RX_HARDWARE
|
1228 SOF_TIMESTAMPING_RAW_HARDWARE
;
1229 info
->phc_index
= ptp_clock_index(dp83640
->clock
->ptp_clock
);
1231 (1 << HWTSTAMP_TX_OFF
) |
1232 (1 << HWTSTAMP_TX_ON
) |
1233 (1 << HWTSTAMP_TX_ONESTEP_SYNC
);
1235 (1 << HWTSTAMP_FILTER_NONE
) |
1236 (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT
) |
1237 (1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC
) |
1238 (1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ
) |
1239 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT
) |
1240 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC
) |
1241 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ
) |
1242 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT
) |
1243 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC
) |
1244 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ
) |
1245 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT
) |
1246 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC
) |
1247 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ
);
1251 static struct phy_driver dp83640_driver
= {
1252 .phy_id
= DP83640_PHY_ID
,
1253 .phy_id_mask
= 0xfffffff0,
1254 .name
= "NatSemi DP83640",
1255 .features
= PHY_BASIC_FEATURES
,
1257 .probe
= dp83640_probe
,
1258 .remove
= dp83640_remove
,
1259 .config_aneg
= genphy_config_aneg
,
1260 .read_status
= genphy_read_status
,
1261 .ts_info
= dp83640_ts_info
,
1262 .hwtstamp
= dp83640_hwtstamp
,
1263 .rxtstamp
= dp83640_rxtstamp
,
1264 .txtstamp
= dp83640_txtstamp
,
1265 .driver
= {.owner
= THIS_MODULE
,}
1268 static int __init
dp83640_init(void)
1270 return phy_driver_register(&dp83640_driver
);
1273 static void __exit
dp83640_exit(void)
1275 dp83640_free_clocks();
1276 phy_driver_unregister(&dp83640_driver
);
1279 MODULE_DESCRIPTION("National Semiconductor DP83640 PHY driver");
1280 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.at>");
1281 MODULE_LICENSE("GPL");
1283 module_init(dp83640_init
);
1284 module_exit(dp83640_exit
);
1286 static struct mdio_device_id __maybe_unused dp83640_tbl
[] = {
1287 { DP83640_PHY_ID
, 0xfffffff0 },
1291 MODULE_DEVICE_TABLE(mdio
, dp83640_tbl
);