dp83640.c

   1// SPDX-License-Identifier: GPL-2.0+
   2/*
   3 * Driver for the National Semiconductor DP83640 PHYTER
   4 *
   5 * Copyright (C) 2010 OMICRON electronics GmbH
   6 */
   7
   8#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   9
  10#include <linux/crc32.h>
  11#include <linux/ethtool.h>
  12#include <linux/kernel.h>
  13#include <linux/list.h>
  14#include <linux/mii.h>
  15#include <linux/module.h>
  16#include <linux/net_tstamp.h>
  17#include <linux/netdevice.h>
  18#include <linux/if_vlan.h>
  19#include <linux/phy.h>
  20#include <linux/ptp_classify.h>
  21#include <linux/ptp_clock_kernel.h>
  22
  23#include "dp83640_reg.h"
  24
  25#define DP83640_PHY_ID	0x20005ce1
  26#define PAGESEL		0x13
  27#define MAX_RXTS	64
  28#define N_EXT_TS	6
  29#define N_PER_OUT	7
  30#define PSF_PTPVER	2
  31#define PSF_EVNT	0x4000
  32#define PSF_RX		0x2000
  33#define PSF_TX		0x1000
  34#define EXT_EVENT	1
  35#define CAL_EVENT	7
  36#define CAL_TRIGGER	1
  37#define DP83640_N_PINS	12
  38
  39#define MII_DP83640_MICR 0x11
  40#define MII_DP83640_MISR 0x12
  41
  42#define MII_DP83640_MICR_OE 0x1
  43#define MII_DP83640_MICR_IE 0x2
  44
  45#define MII_DP83640_MISR_RHF_INT_EN 0x01
  46#define MII_DP83640_MISR_FHF_INT_EN 0x02
  47#define MII_DP83640_MISR_ANC_INT_EN 0x04
  48#define MII_DP83640_MISR_DUP_INT_EN 0x08
  49#define MII_DP83640_MISR_SPD_INT_EN 0x10
  50#define MII_DP83640_MISR_LINK_INT_EN 0x20
  51#define MII_DP83640_MISR_ED_INT_EN 0x40
  52#define MII_DP83640_MISR_LQ_INT_EN 0x80
  53#define MII_DP83640_MISR_ANC_INT 0x400
  54#define MII_DP83640_MISR_DUP_INT 0x800
  55#define MII_DP83640_MISR_SPD_INT 0x1000
  56#define MII_DP83640_MISR_LINK_INT 0x2000
  57#define MII_DP83640_MISR_INT_MASK (MII_DP83640_MISR_ANC_INT |\
  58				   MII_DP83640_MISR_DUP_INT |\
  59				   MII_DP83640_MISR_SPD_INT |\
  60				   MII_DP83640_MISR_LINK_INT)
  61
  62/* phyter seems to miss the mark by 16 ns */
  63#define ADJTIME_FIX	16
  64
  65#define SKB_TIMESTAMP_TIMEOUT	2 /* jiffies */
  66
  67#if defined(__BIG_ENDIAN)
  68#define ENDIAN_FLAG	0
  69#elif defined(__LITTLE_ENDIAN)
  70#define ENDIAN_FLAG	PSF_ENDIAN
  71#endif
  72
  73struct dp83640_skb_info {
  74	int ptp_type;
  75	unsigned long tmo;
  76};
  77
  78struct phy_rxts {
  79	u16 ns_lo;   /* ns[15:0] */
  80	u16 ns_hi;   /* overflow[1:0], ns[29:16] */
  81	u16 sec_lo;  /* sec[15:0] */
  82	u16 sec_hi;  /* sec[31:16] */
  83	u16 seqid;   /* sequenceId[15:0] */
  84	u16 msgtype; /* messageType[3:0], hash[11:0] */
  85};
  86
  87struct phy_txts {
  88	u16 ns_lo;   /* ns[15:0] */
  89	u16 ns_hi;   /* overflow[1:0], ns[29:16] */
  90	u16 sec_lo;  /* sec[15:0] */
  91	u16 sec_hi;  /* sec[31:16] */
  92};
  93
  94struct rxts {
  95	struct list_head list;
  96	unsigned long tmo;
  97	u64 ns;
  98	u16 seqid;
  99	u8  msgtype;
 100	u16 hash;
 101};
 102
 103struct dp83640_clock;
 104
 105struct dp83640_private {
 106	struct list_head list;
 107	struct dp83640_clock *clock;
 108	struct phy_device *phydev;
 109	struct mii_timestamper mii_ts;
 110	struct delayed_work ts_work;
 111	int hwts_tx_en;
 112	int hwts_rx_en;
 113	int layer;
 114	int version;
 115	/* remember state of cfg0 during calibration */
 116	int cfg0;
 117	/* remember the last event time stamp */
 118	struct phy_txts edata;
 119	/* list of rx timestamps */
 120	struct list_head rxts;
 121	struct list_head rxpool;
 122	struct rxts rx_pool_data[MAX_RXTS];
 123	/* protects above three fields from concurrent access */
 124	spinlock_t rx_lock;
 125	/* queues of incoming and outgoing packets */
 126	struct sk_buff_head rx_queue;
 127	struct sk_buff_head tx_queue;
 128};
 129
 130struct dp83640_clock {
 131	/* keeps the instance in the 'phyter_clocks' list */
 132	struct list_head list;
 133	/* we create one clock instance per MII bus */
 134	struct mii_bus *bus;
 135	/* protects extended registers from concurrent access */
 136	struct mutex extreg_lock;
 137	/* remembers which page was last selected */
 138	int page;
 139	/* our advertised capabilities */
 140	struct ptp_clock_info caps;
 141	/* protects the three fields below from concurrent access */
 142	struct mutex clock_lock;
 143	/* the one phyter from which we shall read */
 144	struct dp83640_private *chosen;
 145	/* list of the other attached phyters, not chosen */
 146	struct list_head phylist;
 147	/* reference to our PTP hardware clock */
 148	struct ptp_clock *ptp_clock;
 149};
 150
 151/* globals */
 152
 153enum {
 154	CALIBRATE_GPIO,
 155	PEROUT_GPIO,
 156	EXTTS0_GPIO,
 157	EXTTS1_GPIO,
 158	EXTTS2_GPIO,
 159	EXTTS3_GPIO,
 160	EXTTS4_GPIO,
 161	EXTTS5_GPIO,
 162	GPIO_TABLE_SIZE
 163};
 164
 165static int chosen_phy = -1;
 166static ushort gpio_tab[GPIO_TABLE_SIZE] = {
 167	1, 2, 3, 4, 8, 9, 10, 11
 168};
 169
 170module_param(chosen_phy, int, 0444);
 171module_param_array(gpio_tab, ushort, NULL, 0444);
 172
 173MODULE_PARM_DESC(chosen_phy,
 174	"The address of the PHY to use for the ancillary clock features");
 175MODULE_PARM_DESC(gpio_tab,
 176	"Which GPIO line to use for which purpose: cal,perout,extts1,...,extts6");
 177
 178static void dp83640_gpio_defaults(struct ptp_pin_desc *pd)
 179{
 180	int i, index;
 181
 182	for (i = 0; i < DP83640_N_PINS; i++) {
 183		snprintf(pd[i].name, sizeof(pd[i].name), "GPIO%d", 1 + i);
 184		pd[i].index = i;
 185	}
 186
 187	for (i = 0; i < GPIO_TABLE_SIZE; i++) {
 188		if (gpio_tab[i] < 1 || gpio_tab[i] > DP83640_N_PINS) {
 189			pr_err("gpio_tab[%d]=%hu out of range", i, gpio_tab[i]);
 190			return;
 191		}
 192	}
 193
 194	index = gpio_tab[CALIBRATE_GPIO] - 1;
 195	pd[index].func = PTP_PF_PHYSYNC;
 196	pd[index].chan = 0;
 197
 198	index = gpio_tab[PEROUT_GPIO] - 1;
 199	pd[index].func = PTP_PF_PEROUT;
 200	pd[index].chan = 0;
 201
 202	for (i = EXTTS0_GPIO; i < GPIO_TABLE_SIZE; i++) {
 203		index = gpio_tab[i] - 1;
 204		pd[index].func = PTP_PF_EXTTS;
 205		pd[index].chan = i - EXTTS0_GPIO;
 206	}
 207}
 208
 209/* a list of clocks and a mutex to protect it */
 210static LIST_HEAD(phyter_clocks);
 211static DEFINE_MUTEX(phyter_clocks_lock);
 212
 213static void rx_timestamp_work(struct work_struct *work);
 214
 215/* extended register access functions */
 216
 217#define BROADCAST_ADDR 31
 218
 219static inline int broadcast_write(struct phy_device *phydev, u32 regnum,
 220				  u16 val)
 221{
 222	return mdiobus_write(phydev->mdio.bus, BROADCAST_ADDR, regnum, val);
 223}
 224
 225/* Caller must hold extreg_lock. */
 226static int ext_read(struct phy_device *phydev, int page, u32 regnum)
 227{
 228	struct dp83640_private *dp83640 = phydev->priv;
 229	int val;
 230
 231	if (dp83640->clock->page != page) {
 232		broadcast_write(phydev, PAGESEL, page);
 233		dp83640->clock->page = page;
 234	}
 235	val = phy_read(phydev, regnum);
 236
 237	return val;
 238}
 239
 240/* Caller must hold extreg_lock. */
 241static void ext_write(int broadcast, struct phy_device *phydev,
 242		      int page, u32 regnum, u16 val)
 243{
 244	struct dp83640_private *dp83640 = phydev->priv;
 245
 246	if (dp83640->clock->page != page) {
 247		broadcast_write(phydev, PAGESEL, page);
 248		dp83640->clock->page = page;
 249	}
 250	if (broadcast)
 251		broadcast_write(phydev, regnum, val);
 252	else
 253		phy_write(phydev, regnum, val);
 254}
 255
 256/* Caller must hold extreg_lock. */
 257static int tdr_write(int bc, struct phy_device *dev,
 258		     const struct timespec64 *ts, u16 cmd)
 259{
 260	ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec & 0xffff);/* ns[15:0]  */
 261	ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec >> 16);   /* ns[31:16] */
 262	ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_sec & 0xffff); /* sec[15:0] */
 263	ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_sec >> 16);    /* sec[31:16]*/
 264
 265	ext_write(bc, dev, PAGE4, PTP_CTL, cmd);
 266
 267	return 0;
 268}
 269
 270/* convert phy timestamps into driver timestamps */
 271
 272static void phy2rxts(struct phy_rxts *p, struct rxts *rxts)
 273{
 274	u32 sec;
 275
 276	sec = p->sec_lo;
 277	sec |= p->sec_hi << 16;
 278
 279	rxts->ns = p->ns_lo;
 280	rxts->ns |= (p->ns_hi & 0x3fff) << 16;
 281	rxts->ns += ((u64)sec) * 1000000000ULL;
 282	rxts->seqid = p->seqid;
 283	rxts->msgtype = (p->msgtype >> 12) & 0xf;
 284	rxts->hash = p->msgtype & 0x0fff;
 285	rxts->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
 286}
 287
 288static u64 phy2txts(struct phy_txts *p)
 289{
 290	u64 ns;
 291	u32 sec;
 292
 293	sec = p->sec_lo;
 294	sec |= p->sec_hi << 16;
 295
 296	ns = p->ns_lo;
 297	ns |= (p->ns_hi & 0x3fff) << 16;
 298	ns += ((u64)sec) * 1000000000ULL;
 299
 300	return ns;
 301}
 302
 303static int periodic_output(struct dp83640_clock *clock,
 304			   struct ptp_clock_request *clkreq, bool on,
 305			   int trigger)
 306{
 307	struct dp83640_private *dp83640 = clock->chosen;
 308	struct phy_device *phydev = dp83640->phydev;
 309	u32 sec, nsec, pwidth;
 310	u16 gpio, ptp_trig, val;
 311
 312	if (on) {
 313		gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PEROUT,
 314					trigger);
 315		if (gpio < 1)
 316			return -EINVAL;
 317	} else {
 318		gpio = 0;
 319	}
 320
 321	ptp_trig = TRIG_WR |
 322		(trigger & TRIG_CSEL_MASK) << TRIG_CSEL_SHIFT |
 323		(gpio & TRIG_GPIO_MASK) << TRIG_GPIO_SHIFT |
 324		TRIG_PER |
 325		TRIG_PULSE;
 326
 327	val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
 328
 329	if (!on) {
 330		val |= TRIG_DIS;
 331		mutex_lock(&clock->extreg_lock);
 332		ext_write(0, phydev, PAGE5, PTP_TRIG, ptp_trig);
 333		ext_write(0, phydev, PAGE4, PTP_CTL, val);
 334		mutex_unlock(&clock->extreg_lock);
 335		return 0;
 336	}
 337
 338	sec = clkreq->perout.start.sec;
 339	nsec = clkreq->perout.start.nsec;
 340	pwidth = clkreq->perout.period.sec * 1000000000UL;
 341	pwidth += clkreq->perout.period.nsec;
 342	pwidth /= 2;
 343
 344	mutex_lock(&clock->extreg_lock);
 345
 346	ext_write(0, phydev, PAGE5, PTP_TRIG, ptp_trig);
 347
 348	/*load trigger*/
 349	val |= TRIG_LOAD;
 350	ext_write(0, phydev, PAGE4, PTP_CTL, val);
 351	ext_write(0, phydev, PAGE4, PTP_TDR, nsec & 0xffff);   /* ns[15:0] */
 352	ext_write(0, phydev, PAGE4, PTP_TDR, nsec >> 16);      /* ns[31:16] */
 353	ext_write(0, phydev, PAGE4, PTP_TDR, sec & 0xffff);    /* sec[15:0] */
 354	ext_write(0, phydev, PAGE4, PTP_TDR, sec >> 16);       /* sec[31:16] */
 355	ext_write(0, phydev, PAGE4, PTP_TDR, pwidth & 0xffff); /* ns[15:0] */
 356	ext_write(0, phydev, PAGE4, PTP_TDR, pwidth >> 16);    /* ns[31:16] */
 357	/* Triggers 0 and 1 has programmable pulsewidth2 */
 358	if (trigger < 2) {
 359		ext_write(0, phydev, PAGE4, PTP_TDR, pwidth & 0xffff);
 360		ext_write(0, phydev, PAGE4, PTP_TDR, pwidth >> 16);
 361	}
 362
 363	/*enable trigger*/
 364	val &= ~TRIG_LOAD;
 365	val |= TRIG_EN;
 366	ext_write(0, phydev, PAGE4, PTP_CTL, val);
 367
 368	mutex_unlock(&clock->extreg_lock);
 369	return 0;
 370}
 371
 372/* ptp clock methods */
 373
 374static int ptp_dp83640_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 375{
 376	struct dp83640_clock *clock =
 377		container_of(ptp, struct dp83640_clock, caps);
 378	struct phy_device *phydev = clock->chosen->phydev;
 379	u64 rate;
 380	int neg_adj = 0;
 381	u16 hi, lo;
 382
 383	if (scaled_ppm < 0) {
 384		neg_adj = 1;
 385		scaled_ppm = -scaled_ppm;
 386	}
 387	rate = scaled_ppm;
 388	rate <<= 13;
 389	rate = div_u64(rate, 15625);
 390
 391	hi = (rate >> 16) & PTP_RATE_HI_MASK;
 392	if (neg_adj)
 393		hi |= PTP_RATE_DIR;
 394
 395	lo = rate & 0xffff;
 396
 397	mutex_lock(&clock->extreg_lock);
 398
 399	ext_write(1, phydev, PAGE4, PTP_RATEH, hi);
 400	ext_write(1, phydev, PAGE4, PTP_RATEL, lo);
 401
 402	mutex_unlock(&clock->extreg_lock);
 403
 404	return 0;
 405}
 406
 407static int ptp_dp83640_adjtime(struct ptp_clock_info *ptp, s64 delta)
 408{
 409	struct dp83640_clock *clock =
 410		container_of(ptp, struct dp83640_clock, caps);
 411	struct phy_device *phydev = clock->chosen->phydev;
 412	struct timespec64 ts;
 413	int err;
 414
 415	delta += ADJTIME_FIX;
 416
 417	ts = ns_to_timespec64(delta);
 418
 419	mutex_lock(&clock->extreg_lock);
 420
 421	err = tdr_write(1, phydev, &ts, PTP_STEP_CLK);
 422
 423	mutex_unlock(&clock->extreg_lock);
 424
 425	return err;
 426}
 427
 428static int ptp_dp83640_gettime(struct ptp_clock_info *ptp,
 429			       struct timespec64 *ts)
 430{
 431	struct dp83640_clock *clock =
 432		container_of(ptp, struct dp83640_clock, caps);
 433	struct phy_device *phydev = clock->chosen->phydev;
 434	unsigned int val[4];
 435
 436	mutex_lock(&clock->extreg_lock);
 437
 438	ext_write(0, phydev, PAGE4, PTP_CTL, PTP_RD_CLK);
 439
 440	val[0] = ext_read(phydev, PAGE4, PTP_TDR); /* ns[15:0] */
 441	val[1] = ext_read(phydev, PAGE4, PTP_TDR); /* ns[31:16] */
 442	val[2] = ext_read(phydev, PAGE4, PTP_TDR); /* sec[15:0] */
 443	val[3] = ext_read(phydev, PAGE4, PTP_TDR); /* sec[31:16] */
 444
 445	mutex_unlock(&clock->extreg_lock);
 446
 447	ts->tv_nsec = val[0] | (val[1] << 16);
 448	ts->tv_sec  = val[2] | (val[3] << 16);
 449
 450	return 0;
 451}
 452
 453static int ptp_dp83640_settime(struct ptp_clock_info *ptp,
 454			       const struct timespec64 *ts)
 455{
 456	struct dp83640_clock *clock =
 457		container_of(ptp, struct dp83640_clock, caps);
 458	struct phy_device *phydev = clock->chosen->phydev;
 459	int err;
 460
 461	mutex_lock(&clock->extreg_lock);
 462
 463	err = tdr_write(1, phydev, ts, PTP_LOAD_CLK);
 464
 465	mutex_unlock(&clock->extreg_lock);
 466
 467	return err;
 468}
 469
 470static int ptp_dp83640_enable(struct ptp_clock_info *ptp,
 471			      struct ptp_clock_request *rq, int on)
 472{
 473	struct dp83640_clock *clock =
 474		container_of(ptp, struct dp83640_clock, caps);
 475	struct phy_device *phydev = clock->chosen->phydev;
 476	unsigned int index;
 477	u16 evnt, event_num, gpio_num;
 478
 479	switch (rq->type) {
 480	case PTP_CLK_REQ_EXTTS:
 481		/* Reject requests to enable time stamping on both edges. */
 482		if ((rq->extts.flags & PTP_STRICT_FLAGS) &&
 483		    (rq->extts.flags & PTP_ENABLE_FEATURE) &&
 484		    (rq->extts.flags & PTP_EXTTS_EDGES) == PTP_EXTTS_EDGES)
 485			return -EOPNOTSUPP;
 486
 487		index = rq->extts.index;
 488		if (index >= N_EXT_TS)
 489			return -EINVAL;
 490		event_num = EXT_EVENT + index;
 491		evnt = EVNT_WR | (event_num & EVNT_SEL_MASK) << EVNT_SEL_SHIFT;
 492		if (on) {
 493			gpio_num = 1 + ptp_find_pin(clock->ptp_clock,
 494						    PTP_PF_EXTTS, index);
 495			if (gpio_num < 1)
 496				return -EINVAL;
 497			evnt |= (gpio_num & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT;
 498			if (rq->extts.flags & PTP_FALLING_EDGE)
 499				evnt |= EVNT_FALL;
 500			else
 501				evnt |= EVNT_RISE;
 502		}
 503		mutex_lock(&clock->extreg_lock);
 504		ext_write(0, phydev, PAGE5, PTP_EVNT, evnt);
 505		mutex_unlock(&clock->extreg_lock);
 506		return 0;
 507
 508	case PTP_CLK_REQ_PEROUT:
 509		if (rq->perout.index >= N_PER_OUT)
 510			return -EINVAL;
 511		return periodic_output(clock, rq, on, rq->perout.index);
 512
 513	default:
 514		break;
 515	}
 516
 517	return -EOPNOTSUPP;
 518}
 519
 520static int ptp_dp83640_verify(struct ptp_clock_info *ptp, unsigned int pin,
 521			      enum ptp_pin_function func, unsigned int chan)
 522{
 523	struct dp83640_clock *clock =
 524		container_of(ptp, struct dp83640_clock, caps);
 525
 526	if (clock->caps.pin_config[pin].func == PTP_PF_PHYSYNC &&
 527	    !list_empty(&clock->phylist))
 528		return 1;
 529
 530	if (func == PTP_PF_PHYSYNC)
 531		return 1;
 532
 533	return 0;
 534}
 535
 536static u8 status_frame_dst[6] = { 0x01, 0x1B, 0x19, 0x00, 0x00, 0x00 };
 537static u8 status_frame_src[6] = { 0x08, 0x00, 0x17, 0x0B, 0x6B, 0x0F };
 538
 539static void enable_status_frames(struct phy_device *phydev, bool on)
 540{
 541	struct dp83640_private *dp83640 = phydev->priv;
 542	struct dp83640_clock *clock = dp83640->clock;
 543	u16 cfg0 = 0, ver;
 544
 545	if (on)
 546		cfg0 = PSF_EVNT_EN | PSF_RXTS_EN | PSF_TXTS_EN | ENDIAN_FLAG;
 547
 548	ver = (PSF_PTPVER & VERSIONPTP_MASK) << VERSIONPTP_SHIFT;
 549
 550	mutex_lock(&clock->extreg_lock);
 551
 552	ext_write(0, phydev, PAGE5, PSF_CFG0, cfg0);
 553	ext_write(0, phydev, PAGE6, PSF_CFG1, ver);
 554
 555	mutex_unlock(&clock->extreg_lock);
 556
 557	if (!phydev->attached_dev) {
 558		phydev_warn(phydev,
 559			    "expected to find an attached netdevice\n");
 560		return;
 561	}
 562
 563	if (on) {
 564		if (dev_mc_add(phydev->attached_dev, status_frame_dst))
 565			phydev_warn(phydev, "failed to add mc address\n");
 566	} else {
 567		if (dev_mc_del(phydev->attached_dev, status_frame_dst))
 568			phydev_warn(phydev, "failed to delete mc address\n");
 569	}
 570}
 571
 572static bool is_status_frame(struct sk_buff *skb, int type)
 573{
 574	struct ethhdr *h = eth_hdr(skb);
 575
 576	if (PTP_CLASS_V2_L2 == type &&
 577	    !memcmp(h->h_source, status_frame_src, sizeof(status_frame_src)))
 578		return true;
 579	else
 580		return false;
 581}
 582
 583static int expired(struct rxts *rxts)
 584{
 585	return time_after(jiffies, rxts->tmo);
 586}
 587
 588/* Caller must hold rx_lock. */
 589static void prune_rx_ts(struct dp83640_private *dp83640)
 590{
 591	struct list_head *this, *next;
 592	struct rxts *rxts;
 593
 594	list_for_each_safe(this, next, &dp83640->rxts) {
 595		rxts = list_entry(this, struct rxts, list);
 596		if (expired(rxts)) {
 597			list_del_init(&rxts->list);
 598			list_add(&rxts->list, &dp83640->rxpool);
 599		}
 600	}
 601}
 602
 603/* synchronize the phyters so they act as one clock */
 604
 605static void enable_broadcast(struct phy_device *phydev, int init_page, int on)
 606{
 607	int val;
 608
 609	phy_write(phydev, PAGESEL, 0);
 610	val = phy_read(phydev, PHYCR2);
 611	if (on)
 612		val |= BC_WRITE;
 613	else
 614		val &= ~BC_WRITE;
 615	phy_write(phydev, PHYCR2, val);
 616	phy_write(phydev, PAGESEL, init_page);
 617}
 618
 619static void recalibrate(struct dp83640_clock *clock)
 620{
 621	s64 now, diff;
 622	struct phy_txts event_ts;
 623	struct timespec64 ts;
 624	struct dp83640_private *tmp;
 625	struct phy_device *master = clock->chosen->phydev;
 626	u16 cal_gpio, cfg0, evnt, ptp_trig, trigger, val;
 627
 628	trigger = CAL_TRIGGER;
 629	cal_gpio = 1 + ptp_find_pin_unlocked(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
 630	if (cal_gpio < 1) {
 631		pr_err("PHY calibration pin not available - PHY is not calibrated.");
 632		return;
 633	}
 634
 635	mutex_lock(&clock->extreg_lock);
 636
 637	/*
 638	 * enable broadcast, disable status frames, enable ptp clock
 639	 */
 640	list_for_each_entry(tmp, &clock->phylist, list) {
 641		enable_broadcast(tmp->phydev, clock->page, 1);
 642		tmp->cfg0 = ext_read(tmp->phydev, PAGE5, PSF_CFG0);
 643		ext_write(0, tmp->phydev, PAGE5, PSF_CFG0, 0);
 644		ext_write(0, tmp->phydev, PAGE4, PTP_CTL, PTP_ENABLE);
 645	}
 646	enable_broadcast(master, clock->page, 1);
 647	cfg0 = ext_read(master, PAGE5, PSF_CFG0);
 648	ext_write(0, master, PAGE5, PSF_CFG0, 0);
 649	ext_write(0, master, PAGE4, PTP_CTL, PTP_ENABLE);
 650
 651	/*
 652	 * enable an event timestamp
 653	 */
 654	evnt = EVNT_WR | EVNT_RISE | EVNT_SINGLE;
 655	evnt |= (CAL_EVENT & EVNT_SEL_MASK) << EVNT_SEL_SHIFT;
 656	evnt |= (cal_gpio & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT;
 657
 658	list_for_each_entry(tmp, &clock->phylist, list)
 659		ext_write(0, tmp->phydev, PAGE5, PTP_EVNT, evnt);
 660	ext_write(0, master, PAGE5, PTP_EVNT, evnt);
 661
 662	/*
 663	 * configure a trigger
 664	 */
 665	ptp_trig = TRIG_WR | TRIG_IF_LATE | TRIG_PULSE;
 666	ptp_trig |= (trigger  & TRIG_CSEL_MASK) << TRIG_CSEL_SHIFT;
 667	ptp_trig |= (cal_gpio & TRIG_GPIO_MASK) << TRIG_GPIO_SHIFT;
 668	ext_write(0, master, PAGE5, PTP_TRIG, ptp_trig);
 669
 670	/* load trigger */
 671	val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
 672	val |= TRIG_LOAD;
 673	ext_write(0, master, PAGE4, PTP_CTL, val);
 674
 675	/* enable trigger */
 676	val &= ~TRIG_LOAD;
 677	val |= TRIG_EN;
 678	ext_write(0, master, PAGE4, PTP_CTL, val);
 679
 680	/* disable trigger */
 681	val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
 682	val |= TRIG_DIS;
 683	ext_write(0, master, PAGE4, PTP_CTL, val);
 684
 685	/*
 686	 * read out and correct offsets
 687	 */
 688	val = ext_read(master, PAGE4, PTP_STS);
 689	phydev_info(master, "master PTP_STS  0x%04hx\n", val);
 690	val = ext_read(master, PAGE4, PTP_ESTS);
 691	phydev_info(master, "master PTP_ESTS 0x%04hx\n", val);
 692	event_ts.ns_lo  = ext_read(master, PAGE4, PTP_EDATA);
 693	event_ts.ns_hi  = ext_read(master, PAGE4, PTP_EDATA);
 694	event_ts.sec_lo = ext_read(master, PAGE4, PTP_EDATA);
 695	event_ts.sec_hi = ext_read(master, PAGE4, PTP_EDATA);
 696	now = phy2txts(&event_ts);
 697
 698	list_for_each_entry(tmp, &clock->phylist, list) {
 699		val = ext_read(tmp->phydev, PAGE4, PTP_STS);
 700		phydev_info(tmp->phydev, "slave  PTP_STS  0x%04hx\n", val);
 701		val = ext_read(tmp->phydev, PAGE4, PTP_ESTS);
 702		phydev_info(tmp->phydev, "slave  PTP_ESTS 0x%04hx\n", val);
 703		event_ts.ns_lo  = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
 704		event_ts.ns_hi  = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
 705		event_ts.sec_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
 706		event_ts.sec_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
 707		diff = now - (s64) phy2txts(&event_ts);
 708		phydev_info(tmp->phydev, "slave offset %lld nanoseconds\n",
 709			    diff);
 710		diff += ADJTIME_FIX;
 711		ts = ns_to_timespec64(diff);
 712		tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK);
 713	}
 714
 715	/*
 716	 * restore status frames
 717	 */
 718	list_for_each_entry(tmp, &clock->phylist, list)
 719		ext_write(0, tmp->phydev, PAGE5, PSF_CFG0, tmp->cfg0);
 720	ext_write(0, master, PAGE5, PSF_CFG0, cfg0);
 721
 722	mutex_unlock(&clock->extreg_lock);
 723}
 724
 725/* time stamping methods */
 726
 727static inline u16 exts_chan_to_edata(int ch)
 728{
 729	return 1 << ((ch + EXT_EVENT) * 2);
 730}
 731
 732static int decode_evnt(struct dp83640_private *dp83640,
 733		       void *data, int len, u16 ests)
 734{
 735	struct phy_txts *phy_txts;
 736	struct ptp_clock_event event;
 737	int i, parsed;
 738	int words = (ests >> EVNT_TS_LEN_SHIFT) & EVNT_TS_LEN_MASK;
 739	u16 ext_status = 0;
 740
 741	/* calculate length of the event timestamp status message */
 742	if (ests & MULT_EVNT)
 743		parsed = (words + 2) * sizeof(u16);
 744	else
 745		parsed = (words + 1) * sizeof(u16);
 746
 747	/* check if enough data is available */
 748	if (len < parsed)
 749		return len;
 750
 751	if (ests & MULT_EVNT) {
 752		ext_status = *(u16 *) data;
 753		data += sizeof(ext_status);
 754	}
 755
 756	phy_txts = data;
 757
 758	switch (words) {
 759	case 3:
 760		dp83640->edata.sec_hi = phy_txts->sec_hi;
 761		fallthrough;
 762	case 2:
 763		dp83640->edata.sec_lo = phy_txts->sec_lo;
 764		fallthrough;
 765	case 1:
 766		dp83640->edata.ns_hi = phy_txts->ns_hi;
 767		fallthrough;
 768	case 0:
 769		dp83640->edata.ns_lo = phy_txts->ns_lo;
 770	}
 771
 772	if (!ext_status) {
 773		i = ((ests >> EVNT_NUM_SHIFT) & EVNT_NUM_MASK) - EXT_EVENT;
 774		ext_status = exts_chan_to_edata(i);
 775	}
 776
 777	event.type = PTP_CLOCK_EXTTS;
 778	event.timestamp = phy2txts(&dp83640->edata);
 779
 780	/* Compensate for input path and synchronization delays */
 781	event.timestamp -= 35;
 782
 783	for (i = 0; i < N_EXT_TS; i++) {
 784		if (ext_status & exts_chan_to_edata(i)) {
 785			event.index = i;
 786			ptp_clock_event(dp83640->clock->ptp_clock, &event);
 787		}
 788	}
 789
 790	return parsed;
 791}
 792
 793#define DP83640_PACKET_HASH_LEN		10
 794
 795static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
 796{
 797	struct ptp_header *hdr;
 798	u8 msgtype;
 799	u16 seqid;
 800	u16 hash;
 801
 802	/* check sequenceID, messageType, 12 bit hash of offset 20-29 */
 803
 804	hdr = ptp_parse_header(skb, type);
 805	if (!hdr)
 806		return 0;
 807
 808	msgtype = ptp_get_msgtype(hdr, type);
 809
 810	if (rxts->msgtype != (msgtype & 0xf))
 811		return 0;
 812
 813	seqid = be16_to_cpu(hdr->sequence_id);
 814	if (rxts->seqid != seqid)
 815		return 0;
 816
 817	hash = ether_crc(DP83640_PACKET_HASH_LEN,
 818			 (unsigned char *)&hdr->source_port_identity) >> 20;
 819	if (rxts->hash != hash)
 820		return 0;
 821
 822	return 1;
 823}
 824
 825static void decode_rxts(struct dp83640_private *dp83640,
 826			struct phy_rxts *phy_rxts)
 827{
 828	struct rxts *rxts;
 829	struct skb_shared_hwtstamps *shhwtstamps = NULL;
 830	struct sk_buff *skb;
 831	unsigned long flags;
 832	u8 overflow;
 833
 834	overflow = (phy_rxts->ns_hi >> 14) & 0x3;
 835	if (overflow)
 836		pr_debug("rx timestamp queue overflow, count %d\n", overflow);
 837
 838	spin_lock_irqsave(&dp83640->rx_lock, flags);
 839
 840	prune_rx_ts(dp83640);
 841
 842	if (list_empty(&dp83640->rxpool)) {
 843		pr_debug("rx timestamp pool is empty\n");
 844		goto out;
 845	}
 846	rxts = list_first_entry(&dp83640->rxpool, struct rxts, list);
 847	list_del_init(&rxts->list);
 848	phy2rxts(phy_rxts, rxts);
 849
 850	spin_lock(&dp83640->rx_queue.lock);
 851	skb_queue_walk(&dp83640->rx_queue, skb) {
 852		struct dp83640_skb_info *skb_info;
 853
 854		skb_info = (struct dp83640_skb_info *)skb->cb;
 855		if (match(skb, skb_info->ptp_type, rxts)) {
 856			__skb_unlink(skb, &dp83640->rx_queue);
 857			shhwtstamps = skb_hwtstamps(skb);
 858			memset(shhwtstamps, 0, sizeof(*shhwtstamps));
 859			shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns);
 860			list_add(&rxts->list, &dp83640->rxpool);
 861			break;
 862		}
 863	}
 864	spin_unlock(&dp83640->rx_queue.lock);
 865
 866	if (!shhwtstamps)
 867		list_add_tail(&rxts->list, &dp83640->rxts);
 868out:
 869	spin_unlock_irqrestore(&dp83640->rx_lock, flags);
 870
 871	if (shhwtstamps)
 872		netif_rx(skb);
 873}
 874
 875static void decode_txts(struct dp83640_private *dp83640,
 876			struct phy_txts *phy_txts)
 877{
 878	struct skb_shared_hwtstamps shhwtstamps;
 879	struct dp83640_skb_info *skb_info;
 880	struct sk_buff *skb;
 881	u8 overflow;
 882	u64 ns;
 883
 884	/* We must already have the skb that triggered this. */
 885again:
 886	skb = skb_dequeue(&dp83640->tx_queue);
 887	if (!skb) {
 888		pr_debug("have timestamp but tx_queue empty\n");
 889		return;
 890	}
 891
 892	overflow = (phy_txts->ns_hi >> 14) & 0x3;
 893	if (overflow) {
 894		pr_debug("tx timestamp queue overflow, count %d\n", overflow);
 895		while (skb) {
 896			kfree_skb(skb);
 897			skb = skb_dequeue(&dp83640->tx_queue);
 898		}
 899		return;
 900	}
 901	skb_info = (struct dp83640_skb_info *)skb->cb;
 902	if (time_after(jiffies, skb_info->tmo)) {
 903		kfree_skb(skb);
 904		goto again;
 905	}
 906
 907	ns = phy2txts(phy_txts);
 908	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 909	shhwtstamps.hwtstamp = ns_to_ktime(ns);
 910	skb_complete_tx_timestamp(skb, &shhwtstamps);
 911}
 912
 913static void decode_status_frame(struct dp83640_private *dp83640,
 914				struct sk_buff *skb)
 915{
 916	struct phy_rxts *phy_rxts;
 917	struct phy_txts *phy_txts;
 918	u8 *ptr;
 919	int len, size;
 920	u16 ests, type;
 921
 922	ptr = skb->data + 2;
 923
 924	for (len = skb_headlen(skb) - 2; len > sizeof(type); len -= size) {
 925
 926		type = *(u16 *)ptr;
 927		ests = type & 0x0fff;
 928		type = type & 0xf000;
 929		len -= sizeof(type);
 930		ptr += sizeof(type);
 931
 932		if (PSF_RX == type && len >= sizeof(*phy_rxts)) {
 933
 934			phy_rxts = (struct phy_rxts *) ptr;
 935			decode_rxts(dp83640, phy_rxts);
 936			size = sizeof(*phy_rxts);
 937
 938		} else if (PSF_TX == type && len >= sizeof(*phy_txts)) {
 939
 940			phy_txts = (struct phy_txts *) ptr;
 941			decode_txts(dp83640, phy_txts);
 942			size = sizeof(*phy_txts);
 943
 944		} else if (PSF_EVNT == type) {
 945
 946			size = decode_evnt(dp83640, ptr, len, ests);
 947
 948		} else {
 949			size = 0;
 950			break;
 951		}
 952		ptr += size;
 953	}
 954}
 955
 956static void dp83640_clock_init(struct dp83640_clock *clock, struct mii_bus *bus)
 957{
 958	INIT_LIST_HEAD(&clock->list);
 959	clock->bus = bus;
 960	mutex_init(&clock->extreg_lock);
 961	mutex_init(&clock->clock_lock);
 962	INIT_LIST_HEAD(&clock->phylist);
 963	clock->caps.owner = THIS_MODULE;
 964	sprintf(clock->caps.name, "dp83640 timer");
 965	clock->caps.max_adj	= 1953124;
 966	clock->caps.n_alarm	= 0;
 967	clock->caps.n_ext_ts	= N_EXT_TS;
 968	clock->caps.n_per_out	= N_PER_OUT;
 969	clock->caps.n_pins	= DP83640_N_PINS;
 970	clock->caps.pps		= 0;
 971	clock->caps.supported_extts_flags = PTP_RISING_EDGE |
 972					    PTP_FALLING_EDGE |
 973					    PTP_STRICT_FLAGS;
 974	clock->caps.adjfine	= ptp_dp83640_adjfine;
 975	clock->caps.adjtime	= ptp_dp83640_adjtime;
 976	clock->caps.gettime64	= ptp_dp83640_gettime;
 977	clock->caps.settime64	= ptp_dp83640_settime;
 978	clock->caps.enable	= ptp_dp83640_enable;
 979	clock->caps.verify	= ptp_dp83640_verify;
 980	/*
 981	 * Convert the module param defaults into a dynamic pin configuration.
 982	 */
 983	dp83640_gpio_defaults(clock->caps.pin_config);
 984	/*
 985	 * Get a reference to this bus instance.
 986	 */
 987	get_device(&bus->dev);
 988}
 989
 990static int choose_this_phy(struct dp83640_clock *clock,
 991			   struct phy_device *phydev)
 992{
 993	if (chosen_phy == -1 && !clock->chosen)
 994		return 1;
 995
 996	if (chosen_phy == phydev->mdio.addr)
 997		return 1;
 998
 999	return 0;
1000}
1001
1002static struct dp83640_clock *dp83640_clock_get(struct dp83640_clock *clock)
1003{
1004	if (clock)
1005		mutex_lock(&clock->clock_lock);
1006	return clock;
1007}
1008
1009/*
1010 * Look up and lock a clock by bus instance.
1011 * If there is no clock for this bus, then create it first.
1012 */
1013static struct dp83640_clock *dp83640_clock_get_bus(struct mii_bus *bus)
1014{
1015	struct dp83640_clock *clock = NULL, *tmp;
1016	struct list_head *this;
1017
1018	mutex_lock(&phyter_clocks_lock);
1019
1020	list_for_each(this, &phyter_clocks) {
1021		tmp = list_entry(this, struct dp83640_clock, list);
1022		if (tmp->bus == bus) {
1023			clock = tmp;
1024			break;
1025		}
1026	}
1027	if (clock)
1028		goto out;
1029
1030	clock = kzalloc(sizeof(struct dp83640_clock), GFP_KERNEL);
1031	if (!clock)
1032		goto out;
1033
1034	clock->caps.pin_config = kcalloc(DP83640_N_PINS,
1035					 sizeof(struct ptp_pin_desc),
1036					 GFP_KERNEL);
1037	if (!clock->caps.pin_config) {
1038		kfree(clock);
1039		clock = NULL;
1040		goto out;
1041	}
1042	dp83640_clock_init(clock, bus);
1043	list_add_tail(&clock->list, &phyter_clocks);
1044out:
1045	mutex_unlock(&phyter_clocks_lock);
1046
1047	return dp83640_clock_get(clock);
1048}
1049
1050static void dp83640_clock_put(struct dp83640_clock *clock)
1051{
1052	mutex_unlock(&clock->clock_lock);
1053}
1054
1055static int dp83640_soft_reset(struct phy_device *phydev)
1056{
1057	int ret;
1058
1059	ret = genphy_soft_reset(phydev);
1060	if (ret < 0)
1061		return ret;
1062
1063	/* From DP83640 datasheet: "Software driver code must wait 3 us
1064	 * following a software reset before allowing further serial MII
1065	 * operations with the DP83640."
1066	 */
1067	udelay(10);		/* Taking udelay inaccuracy into account */
1068
1069	return 0;
1070}
1071
1072static int dp83640_config_init(struct phy_device *phydev)
1073{
1074	struct dp83640_private *dp83640 = phydev->priv;
1075	struct dp83640_clock *clock = dp83640->clock;
1076
1077	if (clock->chosen && !list_empty(&clock->phylist))
1078		recalibrate(clock);
1079	else {
1080		mutex_lock(&clock->extreg_lock);
1081		enable_broadcast(phydev, clock->page, 1);
1082		mutex_unlock(&clock->extreg_lock);
1083	}
1084
1085	enable_status_frames(phydev, true);
1086
1087	mutex_lock(&clock->extreg_lock);
1088	ext_write(0, phydev, PAGE4, PTP_CTL, PTP_ENABLE);
1089	mutex_unlock(&clock->extreg_lock);
1090
1091	return 0;
1092}
1093
1094static int dp83640_ack_interrupt(struct phy_device *phydev)
1095{
1096	int err = phy_read(phydev, MII_DP83640_MISR);
1097
1098	if (err < 0)
1099		return err;
1100
1101	return 0;
1102}
1103
1104static int dp83640_config_intr(struct phy_device *phydev)
1105{
1106	int micr;
1107	int misr;
1108	int err;
1109
1110	if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
1111		err = dp83640_ack_interrupt(phydev);
1112		if (err)
1113			return err;
1114
1115		misr = phy_read(phydev, MII_DP83640_MISR);
1116		if (misr < 0)
1117			return misr;
1118		misr |=
1119			(MII_DP83640_MISR_ANC_INT_EN |
1120			MII_DP83640_MISR_DUP_INT_EN |
1121			MII_DP83640_MISR_SPD_INT_EN |
1122			MII_DP83640_MISR_LINK_INT_EN);
1123		err = phy_write(phydev, MII_DP83640_MISR, misr);
1124		if (err < 0)
1125			return err;
1126
1127		micr = phy_read(phydev, MII_DP83640_MICR);
1128		if (micr < 0)
1129			return micr;
1130		micr |=
1131			(MII_DP83640_MICR_OE |
1132			MII_DP83640_MICR_IE);
1133		return phy_write(phydev, MII_DP83640_MICR, micr);
1134	} else {
1135		micr = phy_read(phydev, MII_DP83640_MICR);
1136		if (micr < 0)
1137			return micr;
1138		micr &=
1139			~(MII_DP83640_MICR_OE |
1140			MII_DP83640_MICR_IE);
1141		err = phy_write(phydev, MII_DP83640_MICR, micr);
1142		if (err < 0)
1143			return err;
1144
1145		misr = phy_read(phydev, MII_DP83640_MISR);
1146		if (misr < 0)
1147			return misr;
1148		misr &=
1149			~(MII_DP83640_MISR_ANC_INT_EN |
1150			MII_DP83640_MISR_DUP_INT_EN |
1151			MII_DP83640_MISR_SPD_INT_EN |
1152			MII_DP83640_MISR_LINK_INT_EN);
1153		err = phy_write(phydev, MII_DP83640_MISR, misr);
1154		if (err)
1155			return err;
1156
1157		return dp83640_ack_interrupt(phydev);
1158	}
1159}
1160
1161static irqreturn_t dp83640_handle_interrupt(struct phy_device *phydev)
1162{
1163	int irq_status;
1164
1165	irq_status = phy_read(phydev, MII_DP83640_MISR);
1166	if (irq_status < 0) {
1167		phy_error(phydev);
1168		return IRQ_NONE;
1169	}
1170
1171	if (!(irq_status & MII_DP83640_MISR_INT_MASK))
1172		return IRQ_NONE;
1173
1174	phy_trigger_machine(phydev);
1175
1176	return IRQ_HANDLED;
1177}
1178
1179static int dp83640_hwtstamp_get(struct mii_timestamper *mii_ts,
1180				struct kernel_hwtstamp_config *cfg)
1181{
1182	struct dp83640_private *dp83640 =
1183		container_of(mii_ts, struct dp83640_private, mii_ts);
1184
1185	cfg->rx_filter = dp83640->hwts_rx_en;
1186	cfg->tx_type = dp83640->hwts_tx_en;
1187
1188	return 0;
1189}
1190
1191static int dp83640_hwtstamp_set(struct mii_timestamper *mii_ts,
1192				struct kernel_hwtstamp_config *cfg,
1193				struct netlink_ext_ack *extack)
1194{
1195	struct dp83640_private *dp83640 =
1196		container_of(mii_ts, struct dp83640_private, mii_ts);
1197	u16 txcfg0, rxcfg0;
1198
1199	if (cfg->tx_type < 0 || cfg->tx_type > HWTSTAMP_TX_ONESTEP_SYNC)
1200		return -ERANGE;
1201
1202	dp83640->hwts_tx_en = cfg->tx_type;
1203
1204	switch (cfg->rx_filter) {
1205	case HWTSTAMP_FILTER_NONE:
1206		dp83640->hwts_rx_en = 0;
1207		dp83640->layer = 0;
1208		dp83640->version = 0;
1209		break;
1210	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1211	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1212	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1213		dp83640->hwts_rx_en = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
1214		dp83640->layer = PTP_CLASS_L4;
1215		dp83640->version = PTP_CLASS_V1;
1216		cfg->rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
1217		break;
1218	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1219	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1220	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1221		dp83640->hwts_rx_en = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
1222		dp83640->layer = PTP_CLASS_L4;
1223		dp83640->version = PTP_CLASS_V2;
1224		cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
1225		break;
1226	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1227	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1228	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1229		dp83640->hwts_rx_en = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
1230		dp83640->layer = PTP_CLASS_L2;
1231		dp83640->version = PTP_CLASS_V2;
1232		cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
1233		break;
1234	case HWTSTAMP_FILTER_PTP_V2_EVENT:
1235	case HWTSTAMP_FILTER_PTP_V2_SYNC:
1236	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1237		dp83640->hwts_rx_en = HWTSTAMP_FILTER_PTP_V2_EVENT;
1238		dp83640->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
1239		dp83640->version = PTP_CLASS_V2;
1240		cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
1241		break;
1242	default:
1243		return -ERANGE;
1244	}
1245
1246	txcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
1247	rxcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
1248
1249	if (dp83640->layer & PTP_CLASS_L2) {
1250		txcfg0 |= TX_L2_EN;
1251		rxcfg0 |= RX_L2_EN;
1252	}
1253	if (dp83640->layer & PTP_CLASS_L4) {
1254		txcfg0 |= TX_IPV6_EN | TX_IPV4_EN;
1255		rxcfg0 |= RX_IPV6_EN | RX_IPV4_EN;
1256	}
1257
1258	if (dp83640->hwts_tx_en)
1259		txcfg0 |= TX_TS_EN;
1260
1261	if (dp83640->hwts_tx_en == HWTSTAMP_TX_ONESTEP_SYNC)
1262		txcfg0 |= SYNC_1STEP | CHK_1STEP;
1263
1264	if (dp83640->hwts_rx_en)
1265		rxcfg0 |= RX_TS_EN;
1266
1267	mutex_lock(&dp83640->clock->extreg_lock);
1268
1269	ext_write(0, dp83640->phydev, PAGE5, PTP_TXCFG0, txcfg0);
1270	ext_write(0, dp83640->phydev, PAGE5, PTP_RXCFG0, rxcfg0);
1271
1272	mutex_unlock(&dp83640->clock->extreg_lock);
1273
1274	return 0;
1275}
1276
1277static void rx_timestamp_work(struct work_struct *work)
1278{
1279	struct dp83640_private *dp83640 =
1280		container_of(work, struct dp83640_private, ts_work.work);
1281	struct sk_buff *skb;
1282
1283	/* Deliver expired packets. */
1284	while ((skb = skb_dequeue(&dp83640->rx_queue))) {
1285		struct dp83640_skb_info *skb_info;
1286
1287		skb_info = (struct dp83640_skb_info *)skb->cb;
1288		if (!time_after(jiffies, skb_info->tmo)) {
1289			skb_queue_head(&dp83640->rx_queue, skb);
1290			break;
1291		}
1292
1293		netif_rx(skb);
1294	}
1295
1296	if (!skb_queue_empty(&dp83640->rx_queue))
1297		schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
1298}
1299
1300static bool dp83640_rxtstamp(struct mii_timestamper *mii_ts,
1301			     struct sk_buff *skb, int type)
1302{
1303	struct dp83640_private *dp83640 =
1304		container_of(mii_ts, struct dp83640_private, mii_ts);
1305	struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
1306	struct list_head *this, *next;
1307	struct rxts *rxts;
1308	struct skb_shared_hwtstamps *shhwtstamps = NULL;
1309	unsigned long flags;
1310
1311	if (is_status_frame(skb, type)) {
1312		decode_status_frame(dp83640, skb);
1313		kfree_skb(skb);
1314		return true;
1315	}
1316
1317	if (!dp83640->hwts_rx_en)
1318		return false;
1319
1320	if ((type & dp83640->version) == 0 || (type & dp83640->layer) == 0)
1321		return false;
1322
1323	spin_lock_irqsave(&dp83640->rx_lock, flags);
1324	prune_rx_ts(dp83640);
1325	list_for_each_safe(this, next, &dp83640->rxts) {
1326		rxts = list_entry(this, struct rxts, list);
1327		if (match(skb, type, rxts)) {
1328			shhwtstamps = skb_hwtstamps(skb);
1329			memset(shhwtstamps, 0, sizeof(*shhwtstamps));
1330			shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns);
1331			list_del_init(&rxts->list);
1332			list_add(&rxts->list, &dp83640->rxpool);
1333			break;
1334		}
1335	}
1336	spin_unlock_irqrestore(&dp83640->rx_lock, flags);
1337
1338	if (!shhwtstamps) {
1339		skb_info->ptp_type = type;
1340		skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
1341		skb_queue_tail(&dp83640->rx_queue, skb);
1342		schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
1343	} else {
1344		netif_rx(skb);
1345	}
1346
1347	return true;
1348}
1349
1350static void dp83640_txtstamp(struct mii_timestamper *mii_ts,
1351			     struct sk_buff *skb, int type)
1352{
1353	struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
1354	struct dp83640_private *dp83640 =
1355		container_of(mii_ts, struct dp83640_private, mii_ts);
1356
1357	switch (dp83640->hwts_tx_en) {
1358
1359	case HWTSTAMP_TX_ONESTEP_SYNC:
1360		if (ptp_msg_is_sync(skb, type)) {
1361			kfree_skb(skb);
1362			return;
1363		}
1364		fallthrough;
1365	case HWTSTAMP_TX_ON:
1366		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
1367		skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
1368		skb_queue_tail(&dp83640->tx_queue, skb);
1369		break;
1370
1371	case HWTSTAMP_TX_OFF:
1372	default:
1373		kfree_skb(skb);
1374		break;
1375	}
1376}
1377
1378static int dp83640_ts_info(struct mii_timestamper *mii_ts,
1379			   struct kernel_ethtool_ts_info *info)
1380{
1381	struct dp83640_private *dp83640 =
1382		container_of(mii_ts, struct dp83640_private, mii_ts);
1383
1384	info->so_timestamping =
1385		SOF_TIMESTAMPING_TX_HARDWARE |
1386		SOF_TIMESTAMPING_RX_HARDWARE |
1387		SOF_TIMESTAMPING_RAW_HARDWARE;
1388	info->phc_index = ptp_clock_index(dp83640->clock->ptp_clock);
1389	info->tx_types =
1390		(1 << HWTSTAMP_TX_OFF) |
1391		(1 << HWTSTAMP_TX_ON) |
1392		(1 << HWTSTAMP_TX_ONESTEP_SYNC);
1393	info->rx_filters =
1394		(1 << HWTSTAMP_FILTER_NONE) |
1395		(1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
1396		(1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
1397		(1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
1398		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
1399	return 0;
1400}
1401
1402static int dp83640_probe(struct phy_device *phydev)
1403{
1404	struct dp83640_clock *clock;
1405	struct dp83640_private *dp83640;
1406	int err = -ENOMEM, i;
1407
1408	if (phydev->mdio.addr == BROADCAST_ADDR)
1409		return 0;
1410
1411	clock = dp83640_clock_get_bus(phydev->mdio.bus);
1412	if (!clock)
1413		goto no_clock;
1414
1415	dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
1416	if (!dp83640)
1417		goto no_memory;
1418
1419	dp83640->phydev = phydev;
1420	dp83640->mii_ts.rxtstamp = dp83640_rxtstamp;
1421	dp83640->mii_ts.txtstamp = dp83640_txtstamp;
1422	dp83640->mii_ts.hwtstamp_set = dp83640_hwtstamp_set;
1423	dp83640->mii_ts.hwtstamp_get = dp83640_hwtstamp_get;
1424	dp83640->mii_ts.ts_info  = dp83640_ts_info;
1425
1426	INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
1427	INIT_LIST_HEAD(&dp83640->rxts);
1428	INIT_LIST_HEAD(&dp83640->rxpool);
1429	for (i = 0; i < MAX_RXTS; i++)
1430		list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
1431
1432	/* Timestamp selected by default to keep legacy API */
1433	phydev->default_timestamp = true;
1434	phydev->mii_ts = &dp83640->mii_ts;
1435	phydev->priv = dp83640;
1436
1437	spin_lock_init(&dp83640->rx_lock);
1438	skb_queue_head_init(&dp83640->rx_queue);
1439	skb_queue_head_init(&dp83640->tx_queue);
1440
1441	dp83640->clock = clock;
1442
1443	if (choose_this_phy(clock, phydev)) {
1444		clock->chosen = dp83640;
1445		clock->ptp_clock = ptp_clock_register(&clock->caps,
1446						      &phydev->mdio.dev);
1447		if (IS_ERR(clock->ptp_clock)) {
1448			err = PTR_ERR(clock->ptp_clock);
1449			goto no_register;
1450		}
1451	} else
1452		list_add_tail(&dp83640->list, &clock->phylist);
1453
1454	dp83640_clock_put(clock);
1455	return 0;
1456
1457no_register:
1458	clock->chosen = NULL;
1459	kfree(dp83640);
1460no_memory:
1461	dp83640_clock_put(clock);
1462no_clock:
1463	return err;
1464}
1465
1466static void dp83640_remove(struct phy_device *phydev)
1467{
1468	struct dp83640_clock *clock;
1469	struct list_head *this, *next;
1470	struct dp83640_private *tmp, *dp83640 = phydev->priv;
1471	bool remove_clock = false;
1472
1473	if (phydev->mdio.addr == BROADCAST_ADDR)
1474		return;
1475
1476	phydev->mii_ts = NULL;
1477
1478	enable_status_frames(phydev, false);
1479	cancel_delayed_work_sync(&dp83640->ts_work);
1480
1481	skb_queue_purge(&dp83640->rx_queue);
1482	skb_queue_purge(&dp83640->tx_queue);
1483
1484	clock = dp83640_clock_get(dp83640->clock);
1485
1486	if (dp83640 == clock->chosen) {
1487		ptp_clock_unregister(clock->ptp_clock);
1488		clock->chosen = NULL;
1489	} else {
1490		list_for_each_safe(this, next, &clock->phylist) {
1491			tmp = list_entry(this, struct dp83640_private, list);
1492			if (tmp == dp83640) {
1493				list_del_init(&tmp->list);
1494				break;
1495			}
1496		}
1497	}
1498
1499	if (!clock->chosen && list_empty(&clock->phylist))
1500		remove_clock = true;
1501
1502	dp83640_clock_put(clock);
1503	kfree(dp83640);
1504
1505	if (remove_clock) {
1506		mutex_lock(&phyter_clocks_lock);
1507		list_del(&clock->list);
1508		mutex_unlock(&phyter_clocks_lock);
1509
1510		mutex_destroy(&clock->extreg_lock);
1511		mutex_destroy(&clock->clock_lock);
1512		put_device(&clock->bus->dev);
1513		kfree(clock->caps.pin_config);
1514		kfree(clock);
1515	}
1516}
1517
1518static struct phy_driver dp83640_driver[] = {
1519{
1520	.phy_id		= DP83640_PHY_ID,
1521	.phy_id_mask	= 0xfffffff0,
1522	.name		= "NatSemi DP83640",
1523	/* PHY_BASIC_FEATURES */
1524	.probe		= dp83640_probe,
1525	.remove		= dp83640_remove,
1526	.soft_reset	= dp83640_soft_reset,
1527	.config_init	= dp83640_config_init,
1528	.config_intr    = dp83640_config_intr,
1529	.handle_interrupt = dp83640_handle_interrupt,
1530},
1531};
1532
1533module_phy_driver(dp83640_driver);
1534
1535MODULE_DESCRIPTION("National Semiconductor DP83640 PHY driver");
1536MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
1537MODULE_LICENSE("GPL");
1538
1539static const struct mdio_device_id __maybe_unused dp83640_tbl[] = {
1540	{ DP83640_PHY_ID, 0xfffffff0 },
1541	{ }
1542};
1543
1544MODULE_DEVICE_TABLE(mdio, dp83640_tbl);