aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/net/phy/dp83640.c
diff options
context:
space:
mode:
authorGlenn Elliott <gelliott@cs.unc.edu>2012-03-04 19:47:13 -0500
committerGlenn Elliott <gelliott@cs.unc.edu>2012-03-04 19:47:13 -0500
commitc71c03bda1e86c9d5198c5d83f712e695c4f2a1e (patch)
treeecb166cb3e2b7e2adb3b5e292245fefd23381ac8 /drivers/net/phy/dp83640.c
parentea53c912f8a86a8567697115b6a0d8152beee5c8 (diff)
parent6a00f206debf8a5c8899055726ad127dbeeed098 (diff)
Merge branch 'mpi-master' into wip-k-fmlpwip-k-fmlp
Conflicts: litmus/sched_cedf.c
Diffstat (limited to 'drivers/net/phy/dp83640.c')
-rw-r--r--drivers/net/phy/dp83640.c1110
1 files changed, 1110 insertions, 0 deletions
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
new file mode 100644
index 000000000000..2cd8dc5847b4
--- /dev/null
+++ b/drivers/net/phy/dp83640.c
@@ -0,0 +1,1110 @@
1/*
2 * Driver for the National Semiconductor DP83640 PHYTER
3 *
4 * Copyright (C) 2010 OMICRON electronics GmbH
5 *
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.
10 *
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.
15 *
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.
19 */
20#include <linux/ethtool.h>
21#include <linux/kernel.h>
22#include <linux/list.h>
23#include <linux/mii.h>
24#include <linux/module.h>
25#include <linux/net_tstamp.h>
26#include <linux/netdevice.h>
27#include <linux/phy.h>
28#include <linux/ptp_classify.h>
29#include <linux/ptp_clock_kernel.h>
30
31#include "dp83640_reg.h"
32
33#define DP83640_PHY_ID 0x20005ce1
34#define PAGESEL 0x13
35#define LAYER4 0x02
36#define LAYER2 0x01
37#define MAX_RXTS 4
38#define MAX_TXTS 4
39#define N_EXT_TS 1
40#define PSF_PTPVER 2
41#define PSF_EVNT 0x4000
42#define PSF_RX 0x2000
43#define PSF_TX 0x1000
44#define EXT_EVENT 1
45#define EXT_GPIO 1
46#define CAL_EVENT 2
47#define CAL_GPIO 9
48#define CAL_TRIGGER 2
49
50/* phyter seems to miss the mark by 16 ns */
51#define ADJTIME_FIX 16
52
53#if defined(__BIG_ENDIAN)
54#define ENDIAN_FLAG 0
55#elif defined(__LITTLE_ENDIAN)
56#define ENDIAN_FLAG PSF_ENDIAN
57#endif
58
59#define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
60
61struct phy_rxts {
62 u16 ns_lo; /* ns[15:0] */
63 u16 ns_hi; /* overflow[1:0], ns[29:16] */
64 u16 sec_lo; /* sec[15:0] */
65 u16 sec_hi; /* sec[31:16] */
66 u16 seqid; /* sequenceId[15:0] */
67 u16 msgtype; /* messageType[3:0], hash[11:0] */
68};
69
70struct phy_txts {
71 u16 ns_lo; /* ns[15:0] */
72 u16 ns_hi; /* overflow[1:0], ns[29:16] */
73 u16 sec_lo; /* sec[15:0] */
74 u16 sec_hi; /* sec[31:16] */
75};
76
77struct rxts {
78 struct list_head list;
79 unsigned long tmo;
80 u64 ns;
81 u16 seqid;
82 u8 msgtype;
83 u16 hash;
84};
85
86struct dp83640_clock;
87
88struct dp83640_private {
89 struct list_head list;
90 struct dp83640_clock *clock;
91 struct phy_device *phydev;
92 struct work_struct ts_work;
93 int hwts_tx_en;
94 int hwts_rx_en;
95 int layer;
96 int version;
97 /* remember state of cfg0 during calibration */
98 int cfg0;
99 /* remember the last event time stamp */
100 struct phy_txts edata;
101 /* list of rx timestamps */
102 struct list_head rxts;
103 struct list_head rxpool;
104 struct rxts rx_pool_data[MAX_RXTS];
105 /* protects above three fields from concurrent access */
106 spinlock_t rx_lock;
107 /* queues of incoming and outgoing packets */
108 struct sk_buff_head rx_queue;
109 struct sk_buff_head tx_queue;
110};
111
112struct dp83640_clock {
113 /* keeps the instance in the 'phyter_clocks' list */
114 struct list_head list;
115 /* we create one clock instance per MII bus */
116 struct mii_bus *bus;
117 /* protects extended registers from concurrent access */
118 struct mutex extreg_lock;
119 /* remembers which page was last selected */
120 int page;
121 /* our advertised capabilities */
122 struct ptp_clock_info caps;
123 /* protects the three fields below from concurrent access */
124 struct mutex clock_lock;
125 /* the one phyter from which we shall read */
126 struct dp83640_private *chosen;
127 /* list of the other attached phyters, not chosen */
128 struct list_head phylist;
129 /* reference to our PTP hardware clock */
130 struct ptp_clock *ptp_clock;
131};
132
133/* globals */
134
135static int chosen_phy = -1;
136static ushort cal_gpio = 4;
137
138module_param(chosen_phy, int, 0444);
139module_param(cal_gpio, ushort, 0444);
140
141MODULE_PARM_DESC(chosen_phy, \
142 "The address of the PHY to use for the ancillary clock features");
143MODULE_PARM_DESC(cal_gpio, \
144 "Which GPIO line to use for synchronizing multiple PHYs");
145
146/* a list of clocks and a mutex to protect it */
147static LIST_HEAD(phyter_clocks);
148static DEFINE_MUTEX(phyter_clocks_lock);
149
150static void rx_timestamp_work(struct work_struct *work);
151
152/* extended register access functions */
153
154#define BROADCAST_ADDR 31
155
156static inline int broadcast_write(struct mii_bus *bus, u32 regnum, u16 val)
157{
158 return mdiobus_write(bus, BROADCAST_ADDR, regnum, val);
159}
160
161/* Caller must hold extreg_lock. */
162static int ext_read(struct phy_device *phydev, int page, u32 regnum)
163{
164 struct dp83640_private *dp83640 = phydev->priv;
165 int val;
166
167 if (dp83640->clock->page != page) {
168 broadcast_write(phydev->bus, PAGESEL, page);
169 dp83640->clock->page = page;
170 }
171 val = phy_read(phydev, regnum);
172
173 return val;
174}
175
176/* Caller must hold extreg_lock. */
177static void ext_write(int broadcast, struct phy_device *phydev,
178 int page, u32 regnum, u16 val)
179{
180 struct dp83640_private *dp83640 = phydev->priv;
181
182 if (dp83640->clock->page != page) {
183 broadcast_write(phydev->bus, PAGESEL, page);
184 dp83640->clock->page = page;
185 }
186 if (broadcast)
187 broadcast_write(phydev->bus, regnum, val);
188 else
189 phy_write(phydev, regnum, val);
190}
191
192/* Caller must hold extreg_lock. */
193static int tdr_write(int bc, struct phy_device *dev,
194 const struct timespec *ts, u16 cmd)
195{
196 ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec & 0xffff);/* ns[15:0] */
197 ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec >> 16); /* ns[31:16] */
198 ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_sec & 0xffff); /* sec[15:0] */
199 ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_sec >> 16); /* sec[31:16]*/
200
201 ext_write(bc, dev, PAGE4, PTP_CTL, cmd);
202
203 return 0;
204}
205
206/* convert phy timestamps into driver timestamps */
207
208static void phy2rxts(struct phy_rxts *p, struct rxts *rxts)
209{
210 u32 sec;
211
212 sec = p->sec_lo;
213 sec |= p->sec_hi << 16;
214
215 rxts->ns = p->ns_lo;
216 rxts->ns |= (p->ns_hi & 0x3fff) << 16;
217 rxts->ns += ((u64)sec) * 1000000000ULL;
218 rxts->seqid = p->seqid;
219 rxts->msgtype = (p->msgtype >> 12) & 0xf;
220 rxts->hash = p->msgtype & 0x0fff;
221 rxts->tmo = jiffies + HZ;
222}
223
224static u64 phy2txts(struct phy_txts *p)
225{
226 u64 ns;
227 u32 sec;
228
229 sec = p->sec_lo;
230 sec |= p->sec_hi << 16;
231
232 ns = p->ns_lo;
233 ns |= (p->ns_hi & 0x3fff) << 16;
234 ns += ((u64)sec) * 1000000000ULL;
235
236 return ns;
237}
238
239/* ptp clock methods */
240
241static int ptp_dp83640_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
242{
243 struct dp83640_clock *clock =
244 container_of(ptp, struct dp83640_clock, caps);
245 struct phy_device *phydev = clock->chosen->phydev;
246 u64 rate;
247 int neg_adj = 0;
248 u16 hi, lo;
249
250 if (ppb < 0) {
251 neg_adj = 1;
252 ppb = -ppb;
253 }
254 rate = ppb;
255 rate <<= 26;
256 rate = div_u64(rate, 1953125);
257
258 hi = (rate >> 16) & PTP_RATE_HI_MASK;
259 if (neg_adj)
260 hi |= PTP_RATE_DIR;
261
262 lo = rate & 0xffff;
263
264 mutex_lock(&clock->extreg_lock);
265
266 ext_write(1, phydev, PAGE4, PTP_RATEH, hi);
267 ext_write(1, phydev, PAGE4, PTP_RATEL, lo);
268
269 mutex_unlock(&clock->extreg_lock);
270
271 return 0;
272}
273
274static int ptp_dp83640_adjtime(struct ptp_clock_info *ptp, s64 delta)
275{
276 struct dp83640_clock *clock =
277 container_of(ptp, struct dp83640_clock, caps);
278 struct phy_device *phydev = clock->chosen->phydev;
279 struct timespec ts;
280 int err;
281
282 delta += ADJTIME_FIX;
283
284 ts = ns_to_timespec(delta);
285
286 mutex_lock(&clock->extreg_lock);
287
288 err = tdr_write(1, phydev, &ts, PTP_STEP_CLK);
289
290 mutex_unlock(&clock->extreg_lock);
291
292 return err;
293}
294
295static int ptp_dp83640_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
296{
297 struct dp83640_clock *clock =
298 container_of(ptp, struct dp83640_clock, caps);
299 struct phy_device *phydev = clock->chosen->phydev;
300 unsigned int val[4];
301
302 mutex_lock(&clock->extreg_lock);
303
304 ext_write(0, phydev, PAGE4, PTP_CTL, PTP_RD_CLK);
305
306 val[0] = ext_read(phydev, PAGE4, PTP_TDR); /* ns[15:0] */
307 val[1] = ext_read(phydev, PAGE4, PTP_TDR); /* ns[31:16] */
308 val[2] = ext_read(phydev, PAGE4, PTP_TDR); /* sec[15:0] */
309 val[3] = ext_read(phydev, PAGE4, PTP_TDR); /* sec[31:16] */
310
311 mutex_unlock(&clock->extreg_lock);
312
313 ts->tv_nsec = val[0] | (val[1] << 16);
314 ts->tv_sec = val[2] | (val[3] << 16);
315
316 return 0;
317}
318
319static int ptp_dp83640_settime(struct ptp_clock_info *ptp,
320 const struct timespec *ts)
321{
322 struct dp83640_clock *clock =
323 container_of(ptp, struct dp83640_clock, caps);
324 struct phy_device *phydev = clock->chosen->phydev;
325 int err;
326
327 mutex_lock(&clock->extreg_lock);
328
329 err = tdr_write(1, phydev, ts, PTP_LOAD_CLK);
330
331 mutex_unlock(&clock->extreg_lock);
332
333 return err;
334}
335
336static int ptp_dp83640_enable(struct ptp_clock_info *ptp,
337 struct ptp_clock_request *rq, int on)
338{
339 struct dp83640_clock *clock =
340 container_of(ptp, struct dp83640_clock, caps);
341 struct phy_device *phydev = clock->chosen->phydev;
342 u16 evnt;
343
344 switch (rq->type) {
345 case PTP_CLK_REQ_EXTTS:
346 if (rq->extts.index != 0)
347 return -EINVAL;
348 evnt = EVNT_WR | (EXT_EVENT & EVNT_SEL_MASK) << EVNT_SEL_SHIFT;
349 if (on) {
350 evnt |= (EXT_GPIO & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT;
351 evnt |= EVNT_RISE;
352 }
353 ext_write(0, phydev, PAGE5, PTP_EVNT, evnt);
354 return 0;
355 default:
356 break;
357 }
358
359 return -EOPNOTSUPP;
360}
361
362static u8 status_frame_dst[6] = { 0x01, 0x1B, 0x19, 0x00, 0x00, 0x00 };
363static u8 status_frame_src[6] = { 0x08, 0x00, 0x17, 0x0B, 0x6B, 0x0F };
364
365static void enable_status_frames(struct phy_device *phydev, bool on)
366{
367 u16 cfg0 = 0, ver;
368
369 if (on)
370 cfg0 = PSF_EVNT_EN | PSF_RXTS_EN | PSF_TXTS_EN | ENDIAN_FLAG;
371
372 ver = (PSF_PTPVER & VERSIONPTP_MASK) << VERSIONPTP_SHIFT;
373
374 ext_write(0, phydev, PAGE5, PSF_CFG0, cfg0);
375 ext_write(0, phydev, PAGE6, PSF_CFG1, ver);
376
377 if (!phydev->attached_dev) {
378 pr_warning("dp83640: expected to find an attached netdevice\n");
379 return;
380 }
381
382 if (on) {
383 if (dev_mc_add(phydev->attached_dev, status_frame_dst))
384 pr_warning("dp83640: failed to add mc address\n");
385 } else {
386 if (dev_mc_del(phydev->attached_dev, status_frame_dst))
387 pr_warning("dp83640: failed to delete mc address\n");
388 }
389}
390
391static bool is_status_frame(struct sk_buff *skb, int type)
392{
393 struct ethhdr *h = eth_hdr(skb);
394
395 if (PTP_CLASS_V2_L2 == type &&
396 !memcmp(h->h_source, status_frame_src, sizeof(status_frame_src)))
397 return true;
398 else
399 return false;
400}
401
402static int expired(struct rxts *rxts)
403{
404 return time_after(jiffies, rxts->tmo);
405}
406
407/* Caller must hold rx_lock. */
408static void prune_rx_ts(struct dp83640_private *dp83640)
409{
410 struct list_head *this, *next;
411 struct rxts *rxts;
412
413 list_for_each_safe(this, next, &dp83640->rxts) {
414 rxts = list_entry(this, struct rxts, list);
415 if (expired(rxts)) {
416 list_del_init(&rxts->list);
417 list_add(&rxts->list, &dp83640->rxpool);
418 }
419 }
420}
421
422/* synchronize the phyters so they act as one clock */
423
424static void enable_broadcast(struct phy_device *phydev, int init_page, int on)
425{
426 int val;
427 phy_write(phydev, PAGESEL, 0);
428 val = phy_read(phydev, PHYCR2);
429 if (on)
430 val |= BC_WRITE;
431 else
432 val &= ~BC_WRITE;
433 phy_write(phydev, PHYCR2, val);
434 phy_write(phydev, PAGESEL, init_page);
435}
436
437static void recalibrate(struct dp83640_clock *clock)
438{
439 s64 now, diff;
440 struct phy_txts event_ts;
441 struct timespec ts;
442 struct list_head *this;
443 struct dp83640_private *tmp;
444 struct phy_device *master = clock->chosen->phydev;
445 u16 cfg0, evnt, ptp_trig, trigger, val;
446
447 trigger = CAL_TRIGGER;
448
449 mutex_lock(&clock->extreg_lock);
450
451 /*
452 * enable broadcast, disable status frames, enable ptp clock
453 */
454 list_for_each(this, &clock->phylist) {
455 tmp = list_entry(this, struct dp83640_private, list);
456 enable_broadcast(tmp->phydev, clock->page, 1);
457 tmp->cfg0 = ext_read(tmp->phydev, PAGE5, PSF_CFG0);
458 ext_write(0, tmp->phydev, PAGE5, PSF_CFG0, 0);
459 ext_write(0, tmp->phydev, PAGE4, PTP_CTL, PTP_ENABLE);
460 }
461 enable_broadcast(master, clock->page, 1);
462 cfg0 = ext_read(master, PAGE5, PSF_CFG0);
463 ext_write(0, master, PAGE5, PSF_CFG0, 0);
464 ext_write(0, master, PAGE4, PTP_CTL, PTP_ENABLE);
465
466 /*
467 * enable an event timestamp
468 */
469 evnt = EVNT_WR | EVNT_RISE | EVNT_SINGLE;
470 evnt |= (CAL_EVENT & EVNT_SEL_MASK) << EVNT_SEL_SHIFT;
471 evnt |= (cal_gpio & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT;
472
473 list_for_each(this, &clock->phylist) {
474 tmp = list_entry(this, struct dp83640_private, list);
475 ext_write(0, tmp->phydev, PAGE5, PTP_EVNT, evnt);
476 }
477 ext_write(0, master, PAGE5, PTP_EVNT, evnt);
478
479 /*
480 * configure a trigger
481 */
482 ptp_trig = TRIG_WR | TRIG_IF_LATE | TRIG_PULSE;
483 ptp_trig |= (trigger & TRIG_CSEL_MASK) << TRIG_CSEL_SHIFT;
484 ptp_trig |= (cal_gpio & TRIG_GPIO_MASK) << TRIG_GPIO_SHIFT;
485 ext_write(0, master, PAGE5, PTP_TRIG, ptp_trig);
486
487 /* load trigger */
488 val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
489 val |= TRIG_LOAD;
490 ext_write(0, master, PAGE4, PTP_CTL, val);
491
492 /* enable trigger */
493 val &= ~TRIG_LOAD;
494 val |= TRIG_EN;
495 ext_write(0, master, PAGE4, PTP_CTL, val);
496
497 /* disable trigger */
498 val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
499 val |= TRIG_DIS;
500 ext_write(0, master, PAGE4, PTP_CTL, val);
501
502 /*
503 * read out and correct offsets
504 */
505 val = ext_read(master, PAGE4, PTP_STS);
506 pr_info("master PTP_STS 0x%04hx", val);
507 val = ext_read(master, PAGE4, PTP_ESTS);
508 pr_info("master PTP_ESTS 0x%04hx", val);
509 event_ts.ns_lo = ext_read(master, PAGE4, PTP_EDATA);
510 event_ts.ns_hi = ext_read(master, PAGE4, PTP_EDATA);
511 event_ts.sec_lo = ext_read(master, PAGE4, PTP_EDATA);
512 event_ts.sec_hi = ext_read(master, PAGE4, PTP_EDATA);
513 now = phy2txts(&event_ts);
514
515 list_for_each(this, &clock->phylist) {
516 tmp = list_entry(this, struct dp83640_private, list);
517 val = ext_read(tmp->phydev, PAGE4, PTP_STS);
518 pr_info("slave PTP_STS 0x%04hx", val);
519 val = ext_read(tmp->phydev, PAGE4, PTP_ESTS);
520 pr_info("slave PTP_ESTS 0x%04hx", val);
521 event_ts.ns_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
522 event_ts.ns_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
523 event_ts.sec_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
524 event_ts.sec_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
525 diff = now - (s64) phy2txts(&event_ts);
526 pr_info("slave offset %lld nanoseconds\n", diff);
527 diff += ADJTIME_FIX;
528 ts = ns_to_timespec(diff);
529 tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK);
530 }
531
532 /*
533 * restore status frames
534 */
535 list_for_each(this, &clock->phylist) {
536 tmp = list_entry(this, struct dp83640_private, list);
537 ext_write(0, tmp->phydev, PAGE5, PSF_CFG0, tmp->cfg0);
538 }
539 ext_write(0, master, PAGE5, PSF_CFG0, cfg0);
540
541 mutex_unlock(&clock->extreg_lock);
542}
543
544/* time stamping methods */
545
546static int decode_evnt(struct dp83640_private *dp83640,
547 void *data, u16 ests)
548{
549 struct phy_txts *phy_txts;
550 struct ptp_clock_event event;
551 int words = (ests >> EVNT_TS_LEN_SHIFT) & EVNT_TS_LEN_MASK;
552 u16 ext_status = 0;
553
554 if (ests & MULT_EVNT) {
555 ext_status = *(u16 *) data;
556 data += sizeof(ext_status);
557 }
558
559 phy_txts = data;
560
561 switch (words) { /* fall through in every case */
562 case 3:
563 dp83640->edata.sec_hi = phy_txts->sec_hi;
564 case 2:
565 dp83640->edata.sec_lo = phy_txts->sec_lo;
566 case 1:
567 dp83640->edata.ns_hi = phy_txts->ns_hi;
568 case 0:
569 dp83640->edata.ns_lo = phy_txts->ns_lo;
570 }
571
572 event.type = PTP_CLOCK_EXTTS;
573 event.index = 0;
574 event.timestamp = phy2txts(&dp83640->edata);
575
576 ptp_clock_event(dp83640->clock->ptp_clock, &event);
577
578 words = ext_status ? words + 2 : words + 1;
579 return words * sizeof(u16);
580}
581
582static void decode_rxts(struct dp83640_private *dp83640,
583 struct phy_rxts *phy_rxts)
584{
585 struct rxts *rxts;
586 unsigned long flags;
587
588 spin_lock_irqsave(&dp83640->rx_lock, flags);
589
590 prune_rx_ts(dp83640);
591
592 if (list_empty(&dp83640->rxpool)) {
593 pr_warning("dp83640: rx timestamp pool is empty\n");
594 goto out;
595 }
596 rxts = list_first_entry(&dp83640->rxpool, struct rxts, list);
597 list_del_init(&rxts->list);
598 phy2rxts(phy_rxts, rxts);
599 list_add_tail(&rxts->list, &dp83640->rxts);
600out:
601 spin_unlock_irqrestore(&dp83640->rx_lock, flags);
602}
603
604static void decode_txts(struct dp83640_private *dp83640,
605 struct phy_txts *phy_txts)
606{
607 struct skb_shared_hwtstamps shhwtstamps;
608 struct sk_buff *skb;
609 u64 ns;
610
611 /* We must already have the skb that triggered this. */
612
613 skb = skb_dequeue(&dp83640->tx_queue);
614
615 if (!skb) {
616 pr_warning("dp83640: have timestamp but tx_queue empty\n");
617 return;
618 }
619 ns = phy2txts(phy_txts);
620 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
621 shhwtstamps.hwtstamp = ns_to_ktime(ns);
622 skb_complete_tx_timestamp(skb, &shhwtstamps);
623}
624
625static void decode_status_frame(struct dp83640_private *dp83640,
626 struct sk_buff *skb)
627{
628 struct phy_rxts *phy_rxts;
629 struct phy_txts *phy_txts;
630 u8 *ptr;
631 int len, size;
632 u16 ests, type;
633
634 ptr = skb->data + 2;
635
636 for (len = skb_headlen(skb) - 2; len > sizeof(type); len -= size) {
637
638 type = *(u16 *)ptr;
639 ests = type & 0x0fff;
640 type = type & 0xf000;
641 len -= sizeof(type);
642 ptr += sizeof(type);
643
644 if (PSF_RX == type && len >= sizeof(*phy_rxts)) {
645
646 phy_rxts = (struct phy_rxts *) ptr;
647 decode_rxts(dp83640, phy_rxts);
648 size = sizeof(*phy_rxts);
649
650 } else if (PSF_TX == type && len >= sizeof(*phy_txts)) {
651
652 phy_txts = (struct phy_txts *) ptr;
653 decode_txts(dp83640, phy_txts);
654 size = sizeof(*phy_txts);
655
656 } else if (PSF_EVNT == type && len >= sizeof(*phy_txts)) {
657
658 size = decode_evnt(dp83640, ptr, ests);
659
660 } else {
661 size = 0;
662 break;
663 }
664 ptr += size;
665 }
666}
667
668static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
669{
670 u16 *seqid;
671 unsigned int offset;
672 u8 *msgtype, *data = skb_mac_header(skb);
673
674 /* check sequenceID, messageType, 12 bit hash of offset 20-29 */
675
676 switch (type) {
677 case PTP_CLASS_V1_IPV4:
678 case PTP_CLASS_V2_IPV4:
679 offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN;
680 break;
681 case PTP_CLASS_V1_IPV6:
682 case PTP_CLASS_V2_IPV6:
683 offset = OFF_PTP6;
684 break;
685 case PTP_CLASS_V2_L2:
686 offset = ETH_HLEN;
687 break;
688 case PTP_CLASS_V2_VLAN:
689 offset = ETH_HLEN + VLAN_HLEN;
690 break;
691 default:
692 return 0;
693 }
694
695 if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
696 return 0;
697
698 if (unlikely(type & PTP_CLASS_V1))
699 msgtype = data + offset + OFF_PTP_CONTROL;
700 else
701 msgtype = data + offset;
702
703 seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
704
705 return (rxts->msgtype == (*msgtype & 0xf) &&
706 rxts->seqid == ntohs(*seqid));
707}
708
709static void dp83640_free_clocks(void)
710{
711 struct dp83640_clock *clock;
712 struct list_head *this, *next;
713
714 mutex_lock(&phyter_clocks_lock);
715
716 list_for_each_safe(this, next, &phyter_clocks) {
717 clock = list_entry(this, struct dp83640_clock, list);
718 if (!list_empty(&clock->phylist)) {
719 pr_warning("phy list non-empty while unloading");
720 BUG();
721 }
722 list_del(&clock->list);
723 mutex_destroy(&clock->extreg_lock);
724 mutex_destroy(&clock->clock_lock);
725 put_device(&clock->bus->dev);
726 kfree(clock);
727 }
728
729 mutex_unlock(&phyter_clocks_lock);
730}
731
732static void dp83640_clock_init(struct dp83640_clock *clock, struct mii_bus *bus)
733{
734 INIT_LIST_HEAD(&clock->list);
735 clock->bus = bus;
736 mutex_init(&clock->extreg_lock);
737 mutex_init(&clock->clock_lock);
738 INIT_LIST_HEAD(&clock->phylist);
739 clock->caps.owner = THIS_MODULE;
740 sprintf(clock->caps.name, "dp83640 timer");
741 clock->caps.max_adj = 1953124;
742 clock->caps.n_alarm = 0;
743 clock->caps.n_ext_ts = N_EXT_TS;
744 clock->caps.n_per_out = 0;
745 clock->caps.pps = 0;
746 clock->caps.adjfreq = ptp_dp83640_adjfreq;
747 clock->caps.adjtime = ptp_dp83640_adjtime;
748 clock->caps.gettime = ptp_dp83640_gettime;
749 clock->caps.settime = ptp_dp83640_settime;
750 clock->caps.enable = ptp_dp83640_enable;
751 /*
752 * Get a reference to this bus instance.
753 */
754 get_device(&bus->dev);
755}
756
757static int choose_this_phy(struct dp83640_clock *clock,
758 struct phy_device *phydev)
759{
760 if (chosen_phy == -1 && !clock->chosen)
761 return 1;
762
763 if (chosen_phy == phydev->addr)
764 return 1;
765
766 return 0;
767}
768
769static struct dp83640_clock *dp83640_clock_get(struct dp83640_clock *clock)
770{
771 if (clock)
772 mutex_lock(&clock->clock_lock);
773 return clock;
774}
775
776/*
777 * Look up and lock a clock by bus instance.
778 * If there is no clock for this bus, then create it first.
779 */
780static struct dp83640_clock *dp83640_clock_get_bus(struct mii_bus *bus)
781{
782 struct dp83640_clock *clock = NULL, *tmp;
783 struct list_head *this;
784
785 mutex_lock(&phyter_clocks_lock);
786
787 list_for_each(this, &phyter_clocks) {
788 tmp = list_entry(this, struct dp83640_clock, list);
789 if (tmp->bus == bus) {
790 clock = tmp;
791 break;
792 }
793 }
794 if (clock)
795 goto out;
796
797 clock = kzalloc(sizeof(struct dp83640_clock), GFP_KERNEL);
798 if (!clock)
799 goto out;
800
801 dp83640_clock_init(clock, bus);
802 list_add_tail(&phyter_clocks, &clock->list);
803out:
804 mutex_unlock(&phyter_clocks_lock);
805
806 return dp83640_clock_get(clock);
807}
808
809static void dp83640_clock_put(struct dp83640_clock *clock)
810{
811 mutex_unlock(&clock->clock_lock);
812}
813
814static int dp83640_probe(struct phy_device *phydev)
815{
816 struct dp83640_clock *clock;
817 struct dp83640_private *dp83640;
818 int err = -ENOMEM, i;
819
820 if (phydev->addr == BROADCAST_ADDR)
821 return 0;
822
823 clock = dp83640_clock_get_bus(phydev->bus);
824 if (!clock)
825 goto no_clock;
826
827 dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
828 if (!dp83640)
829 goto no_memory;
830
831 dp83640->phydev = phydev;
832 INIT_WORK(&dp83640->ts_work, rx_timestamp_work);
833
834 INIT_LIST_HEAD(&dp83640->rxts);
835 INIT_LIST_HEAD(&dp83640->rxpool);
836 for (i = 0; i < MAX_RXTS; i++)
837 list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
838
839 phydev->priv = dp83640;
840
841 spin_lock_init(&dp83640->rx_lock);
842 skb_queue_head_init(&dp83640->rx_queue);
843 skb_queue_head_init(&dp83640->tx_queue);
844
845 dp83640->clock = clock;
846
847 if (choose_this_phy(clock, phydev)) {
848 clock->chosen = dp83640;
849 clock->ptp_clock = ptp_clock_register(&clock->caps);
850 if (IS_ERR(clock->ptp_clock)) {
851 err = PTR_ERR(clock->ptp_clock);
852 goto no_register;
853 }
854 } else
855 list_add_tail(&dp83640->list, &clock->phylist);
856
857 if (clock->chosen && !list_empty(&clock->phylist))
858 recalibrate(clock);
859 else
860 enable_broadcast(dp83640->phydev, clock->page, 1);
861
862 dp83640_clock_put(clock);
863 return 0;
864
865no_register:
866 clock->chosen = NULL;
867 kfree(dp83640);
868no_memory:
869 dp83640_clock_put(clock);
870no_clock:
871 return err;
872}
873
874static void dp83640_remove(struct phy_device *phydev)
875{
876 struct dp83640_clock *clock;
877 struct list_head *this, *next;
878 struct dp83640_private *tmp, *dp83640 = phydev->priv;
879
880 if (phydev->addr == BROADCAST_ADDR)
881 return;
882
883 enable_status_frames(phydev, false);
884 cancel_work_sync(&dp83640->ts_work);
885
886 clock = dp83640_clock_get(dp83640->clock);
887
888 if (dp83640 == clock->chosen) {
889 ptp_clock_unregister(clock->ptp_clock);
890 clock->chosen = NULL;
891 } else {
892 list_for_each_safe(this, next, &clock->phylist) {
893 tmp = list_entry(this, struct dp83640_private, list);
894 if (tmp == dp83640) {
895 list_del_init(&tmp->list);
896 break;
897 }
898 }
899 }
900
901 dp83640_clock_put(clock);
902 kfree(dp83640);
903}
904
905static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
906{
907 struct dp83640_private *dp83640 = phydev->priv;
908 struct hwtstamp_config cfg;
909 u16 txcfg0, rxcfg0;
910
911 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
912 return -EFAULT;
913
914 if (cfg.flags) /* reserved for future extensions */
915 return -EINVAL;
916
917 switch (cfg.tx_type) {
918 case HWTSTAMP_TX_OFF:
919 dp83640->hwts_tx_en = 0;
920 break;
921 case HWTSTAMP_TX_ON:
922 dp83640->hwts_tx_en = 1;
923 break;
924 default:
925 return -ERANGE;
926 }
927
928 switch (cfg.rx_filter) {
929 case HWTSTAMP_FILTER_NONE:
930 dp83640->hwts_rx_en = 0;
931 dp83640->layer = 0;
932 dp83640->version = 0;
933 break;
934 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
935 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
936 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
937 dp83640->hwts_rx_en = 1;
938 dp83640->layer = LAYER4;
939 dp83640->version = 1;
940 break;
941 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
942 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
943 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
944 dp83640->hwts_rx_en = 1;
945 dp83640->layer = LAYER4;
946 dp83640->version = 2;
947 break;
948 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
949 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
950 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
951 dp83640->hwts_rx_en = 1;
952 dp83640->layer = LAYER2;
953 dp83640->version = 2;
954 break;
955 case HWTSTAMP_FILTER_PTP_V2_EVENT:
956 case HWTSTAMP_FILTER_PTP_V2_SYNC:
957 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
958 dp83640->hwts_rx_en = 1;
959 dp83640->layer = LAYER4|LAYER2;
960 dp83640->version = 2;
961 break;
962 default:
963 return -ERANGE;
964 }
965
966 txcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
967 rxcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
968
969 if (dp83640->layer & LAYER2) {
970 txcfg0 |= TX_L2_EN;
971 rxcfg0 |= RX_L2_EN;
972 }
973 if (dp83640->layer & LAYER4) {
974 txcfg0 |= TX_IPV6_EN | TX_IPV4_EN;
975 rxcfg0 |= RX_IPV6_EN | RX_IPV4_EN;
976 }
977
978 if (dp83640->hwts_tx_en)
979 txcfg0 |= TX_TS_EN;
980
981 if (dp83640->hwts_rx_en)
982 rxcfg0 |= RX_TS_EN;
983
984 mutex_lock(&dp83640->clock->extreg_lock);
985
986 if (dp83640->hwts_tx_en || dp83640->hwts_rx_en) {
987 enable_status_frames(phydev, true);
988 ext_write(0, phydev, PAGE4, PTP_CTL, PTP_ENABLE);
989 }
990
991 ext_write(0, phydev, PAGE5, PTP_TXCFG0, txcfg0);
992 ext_write(0, phydev, PAGE5, PTP_RXCFG0, rxcfg0);
993
994 mutex_unlock(&dp83640->clock->extreg_lock);
995
996 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
997}
998
999static void rx_timestamp_work(struct work_struct *work)
1000{
1001 struct dp83640_private *dp83640 =
1002 container_of(work, struct dp83640_private, ts_work);
1003 struct list_head *this, *next;
1004 struct rxts *rxts;
1005 struct skb_shared_hwtstamps *shhwtstamps;
1006 struct sk_buff *skb;
1007 unsigned int type;
1008 unsigned long flags;
1009
1010 /* Deliver each deferred packet, with or without a time stamp. */
1011
1012 while ((skb = skb_dequeue(&dp83640->rx_queue)) != NULL) {
1013 type = SKB_PTP_TYPE(skb);
1014 spin_lock_irqsave(&dp83640->rx_lock, flags);
1015 list_for_each_safe(this, next, &dp83640->rxts) {
1016 rxts = list_entry(this, struct rxts, list);
1017 if (match(skb, type, rxts)) {
1018 shhwtstamps = skb_hwtstamps(skb);
1019 memset(shhwtstamps, 0, sizeof(*shhwtstamps));
1020 shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns);
1021 list_del_init(&rxts->list);
1022 list_add(&rxts->list, &dp83640->rxpool);
1023 break;
1024 }
1025 }
1026 spin_unlock_irqrestore(&dp83640->rx_lock, flags);
1027 netif_rx(skb);
1028 }
1029
1030 /* Clear out expired time stamps. */
1031
1032 spin_lock_irqsave(&dp83640->rx_lock, flags);
1033 prune_rx_ts(dp83640);
1034 spin_unlock_irqrestore(&dp83640->rx_lock, flags);
1035}
1036
1037static bool dp83640_rxtstamp(struct phy_device *phydev,
1038 struct sk_buff *skb, int type)
1039{
1040 struct dp83640_private *dp83640 = phydev->priv;
1041
1042 if (!dp83640->hwts_rx_en)
1043 return false;
1044
1045 if (is_status_frame(skb, type)) {
1046 decode_status_frame(dp83640, skb);
1047 kfree_skb(skb);
1048 return true;
1049 }
1050
1051 SKB_PTP_TYPE(skb) = type;
1052 skb_queue_tail(&dp83640->rx_queue, skb);
1053 schedule_work(&dp83640->ts_work);
1054
1055 return true;
1056}
1057
1058static void dp83640_txtstamp(struct phy_device *phydev,
1059 struct sk_buff *skb, int type)
1060{
1061 struct dp83640_private *dp83640 = phydev->priv;
1062
1063 if (!dp83640->hwts_tx_en) {
1064 kfree_skb(skb);
1065 return;
1066 }
1067 skb_queue_tail(&dp83640->tx_queue, skb);
1068 schedule_work(&dp83640->ts_work);
1069}
1070
1071static struct phy_driver dp83640_driver = {
1072 .phy_id = DP83640_PHY_ID,
1073 .phy_id_mask = 0xfffffff0,
1074 .name = "NatSemi DP83640",
1075 .features = PHY_BASIC_FEATURES,
1076 .flags = 0,
1077 .probe = dp83640_probe,
1078 .remove = dp83640_remove,
1079 .config_aneg = genphy_config_aneg,
1080 .read_status = genphy_read_status,
1081 .hwtstamp = dp83640_hwtstamp,
1082 .rxtstamp = dp83640_rxtstamp,
1083 .txtstamp = dp83640_txtstamp,
1084 .driver = {.owner = THIS_MODULE,}
1085};
1086
1087static int __init dp83640_init(void)
1088{
1089 return phy_driver_register(&dp83640_driver);
1090}
1091
1092static void __exit dp83640_exit(void)
1093{
1094 dp83640_free_clocks();
1095 phy_driver_unregister(&dp83640_driver);
1096}
1097
1098MODULE_DESCRIPTION("National Semiconductor DP83640 PHY driver");
1099MODULE_AUTHOR("Richard Cochran <richard.cochran@omicron.at>");
1100MODULE_LICENSE("GPL");
1101
1102module_init(dp83640_init);
1103module_exit(dp83640_exit);
1104
1105static struct mdio_device_id __maybe_unused dp83640_tbl[] = {
1106 { DP83640_PHY_ID, 0xfffffff0 },
1107 { }
1108};
1109
1110MODULE_DEVICE_TABLE(mdio, dp83640_tbl);
generated by cgit v1.2.2 (git 2.25.0) at 2025-04-04 10:13:39 -0400