--- zzzz-none-000/linux-3.10.107/drivers/net/phy/dp83640.c 2017-06-27 09:49:32.000000000 +0000 +++ scorpion-7490-727/linux-3.10.107/drivers/net/phy/dp83640.c 2021-02-04 17:41:59.000000000 +0000 @@ -20,6 +20,7 @@ #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt +#include #include #include #include @@ -27,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -35,10 +37,9 @@ #define DP83640_PHY_ID 0x20005ce1 #define PAGESEL 0x13 -#define LAYER4 0x02 -#define LAYER2 0x01 #define MAX_RXTS 64 #define N_EXT_TS 6 +#define N_PER_OUT 7 #define PSF_PTPVER 2 #define PSF_EVNT 0x4000 #define PSF_RX 0x2000 @@ -46,7 +47,7 @@ #define EXT_EVENT 1 #define CAL_EVENT 7 #define CAL_TRIGGER 1 -#define PER_TRIGGER 6 +#define DP83640_N_PINS 12 #define MII_DP83640_MICR 0x11 #define MII_DP83640_MISR 0x12 @@ -66,13 +67,18 @@ /* phyter seems to miss the mark by 16 ns */ #define ADJTIME_FIX 16 +#define SKB_TIMESTAMP_TIMEOUT 2 /* jiffies */ + #if defined(__BIG_ENDIAN) #define ENDIAN_FLAG 0 #elif defined(__LITTLE_ENDIAN) #define ENDIAN_FLAG PSF_ENDIAN #endif -#define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb)) +struct dp83640_skb_info { + int ptp_type; + unsigned long tmo; +}; struct phy_rxts { u16 ns_lo; /* ns[15:0] */ @@ -105,7 +111,7 @@ struct list_head list; struct dp83640_clock *clock; struct phy_device *phydev; - struct work_struct ts_work; + struct delayed_work ts_work; int hwts_tx_en; int hwts_rx_en; int layer; @@ -173,6 +179,37 @@ MODULE_PARM_DESC(gpio_tab, \ "Which GPIO line to use for which purpose: cal,perout,extts1,...,extts6"); +static void dp83640_gpio_defaults(struct ptp_pin_desc *pd) +{ + int i, index; + + for (i = 0; i < DP83640_N_PINS; i++) { + snprintf(pd[i].name, sizeof(pd[i].name), "GPIO%d", 1 + i); + pd[i].index = i; + } + + for (i = 0; i < GPIO_TABLE_SIZE; i++) { + if (gpio_tab[i] < 1 || gpio_tab[i] > DP83640_N_PINS) { + pr_err("gpio_tab[%d]=%hu out of range", i, gpio_tab[i]); + return; + } + } + + index = gpio_tab[CALIBRATE_GPIO] - 1; + pd[index].func = PTP_PF_PHYSYNC; + pd[index].chan = 0; + + index = gpio_tab[PEROUT_GPIO] - 1; + pd[index].func = PTP_PF_PEROUT; + pd[index].chan = 0; + + for (i = EXTTS0_GPIO; i < GPIO_TABLE_SIZE; i++) { + index = gpio_tab[i] - 1; + pd[index].func = PTP_PF_EXTTS; + pd[index].chan = i - EXTTS0_GPIO; + } +} + /* a list of clocks and a mutex to protect it */ static LIST_HEAD(phyter_clocks); static DEFINE_MUTEX(phyter_clocks_lock); @@ -221,7 +258,7 @@ /* Caller must hold extreg_lock. */ static int tdr_write(int bc, struct phy_device *dev, - const struct timespec *ts, u16 cmd) + const struct timespec64 *ts, u16 cmd) { ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec & 0xffff);/* ns[15:0] */ ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec >> 16); /* ns[31:16] */ @@ -248,7 +285,7 @@ rxts->seqid = p->seqid; rxts->msgtype = (p->msgtype >> 12) & 0xf; rxts->hash = p->msgtype & 0x0fff; - rxts->tmo = jiffies + 2; + rxts->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT; } static u64 phy2txts(struct phy_txts *p) @@ -266,16 +303,23 @@ return ns; } -static void periodic_output(struct dp83640_clock *clock, - struct ptp_clock_request *clkreq, bool on) +static int periodic_output(struct dp83640_clock *clock, + struct ptp_clock_request *clkreq, bool on, + int trigger) { struct dp83640_private *dp83640 = clock->chosen; struct phy_device *phydev = dp83640->phydev; - u32 sec, nsec, period; - u16 gpio, ptp_trig, trigger, val; + u32 sec, nsec, pwidth; + u16 gpio, ptp_trig, val; - gpio = on ? gpio_tab[PEROUT_GPIO] : 0; - trigger = PER_TRIGGER; + if (on) { + gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PEROUT, + trigger); + if (gpio < 1) + return -EINVAL; + } else { + gpio = 0; + } ptp_trig = TRIG_WR | (trigger & TRIG_CSEL_MASK) << TRIG_CSEL_SHIFT | @@ -291,13 +335,14 @@ ext_write(0, phydev, PAGE5, PTP_TRIG, ptp_trig); ext_write(0, phydev, PAGE4, PTP_CTL, val); mutex_unlock(&clock->extreg_lock); - return; + return 0; } sec = clkreq->perout.start.sec; nsec = clkreq->perout.start.nsec; - period = clkreq->perout.period.sec * 1000000000UL; - period += clkreq->perout.period.nsec; + pwidth = clkreq->perout.period.sec * 1000000000UL; + pwidth += clkreq->perout.period.nsec; + pwidth /= 2; mutex_lock(&clock->extreg_lock); @@ -310,8 +355,13 @@ ext_write(0, phydev, PAGE4, PTP_TDR, nsec >> 16); /* ns[31:16] */ ext_write(0, phydev, PAGE4, PTP_TDR, sec & 0xffff); /* sec[15:0] */ ext_write(0, phydev, PAGE4, PTP_TDR, sec >> 16); /* sec[31:16] */ - ext_write(0, phydev, PAGE4, PTP_TDR, period & 0xffff); /* ns[15:0] */ - ext_write(0, phydev, PAGE4, PTP_TDR, period >> 16); /* ns[31:16] */ + ext_write(0, phydev, PAGE4, PTP_TDR, pwidth & 0xffff); /* ns[15:0] */ + ext_write(0, phydev, PAGE4, PTP_TDR, pwidth >> 16); /* ns[31:16] */ + /* Triggers 0 and 1 has programmable pulsewidth2 */ + if (trigger < 2) { + ext_write(0, phydev, PAGE4, PTP_TDR, pwidth & 0xffff); + ext_write(0, phydev, PAGE4, PTP_TDR, pwidth >> 16); + } /*enable trigger*/ val &= ~TRIG_LOAD; @@ -319,6 +369,7 @@ ext_write(0, phydev, PAGE4, PTP_CTL, val); mutex_unlock(&clock->extreg_lock); + return 0; } /* ptp clock methods */ @@ -361,12 +412,12 @@ struct dp83640_clock *clock = container_of(ptp, struct dp83640_clock, caps); struct phy_device *phydev = clock->chosen->phydev; - struct timespec ts; + struct timespec64 ts; int err; delta += ADJTIME_FIX; - ts = ns_to_timespec(delta); + ts = ns_to_timespec64(delta); mutex_lock(&clock->extreg_lock); @@ -377,7 +428,8 @@ return err; } -static int ptp_dp83640_gettime(struct ptp_clock_info *ptp, struct timespec *ts) +static int ptp_dp83640_gettime(struct ptp_clock_info *ptp, + struct timespec64 *ts) { struct dp83640_clock *clock = container_of(ptp, struct dp83640_clock, caps); @@ -402,7 +454,7 @@ } static int ptp_dp83640_settime(struct ptp_clock_info *ptp, - const struct timespec *ts) + const struct timespec64 *ts) { struct dp83640_clock *clock = container_of(ptp, struct dp83640_clock, caps); @@ -424,29 +476,36 @@ struct dp83640_clock *clock = container_of(ptp, struct dp83640_clock, caps); struct phy_device *phydev = clock->chosen->phydev; - int index; + unsigned int index; u16 evnt, event_num, gpio_num; switch (rq->type) { case PTP_CLK_REQ_EXTTS: index = rq->extts.index; - if (index < 0 || index >= N_EXT_TS) + if (index >= N_EXT_TS) return -EINVAL; event_num = EXT_EVENT + index; evnt = EVNT_WR | (event_num & EVNT_SEL_MASK) << EVNT_SEL_SHIFT; if (on) { - gpio_num = gpio_tab[EXTTS0_GPIO + index]; + gpio_num = 1 + ptp_find_pin(clock->ptp_clock, + PTP_PF_EXTTS, index); + if (gpio_num < 1) + return -EINVAL; evnt |= (gpio_num & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT; - evnt |= EVNT_RISE; + if (rq->extts.flags & PTP_FALLING_EDGE) + evnt |= EVNT_FALL; + else + evnt |= EVNT_RISE; } + mutex_lock(&clock->extreg_lock); ext_write(0, phydev, PAGE5, PTP_EVNT, evnt); + mutex_unlock(&clock->extreg_lock); return 0; case PTP_CLK_REQ_PEROUT: - if (rq->perout.index != 0) + if (rq->perout.index >= N_PER_OUT) return -EINVAL; - periodic_output(clock, rq, on); - return 0; + return periodic_output(clock, rq, on, rq->perout.index); default: break; @@ -455,11 +514,29 @@ return -EOPNOTSUPP; } +static int ptp_dp83640_verify(struct ptp_clock_info *ptp, unsigned int pin, + enum ptp_pin_function func, unsigned int chan) +{ + struct dp83640_clock *clock = + container_of(ptp, struct dp83640_clock, caps); + + if (clock->caps.pin_config[pin].func == PTP_PF_PHYSYNC && + !list_empty(&clock->phylist)) + return 1; + + if (func == PTP_PF_PHYSYNC) + return 1; + + return 0; +} + static u8 status_frame_dst[6] = { 0x01, 0x1B, 0x19, 0x00, 0x00, 0x00 }; static u8 status_frame_src[6] = { 0x08, 0x00, 0x17, 0x0B, 0x6B, 0x0F }; static void enable_status_frames(struct phy_device *phydev, bool on) { + struct dp83640_private *dp83640 = phydev->priv; + struct dp83640_clock *clock = dp83640->clock; u16 cfg0 = 0, ver; if (on) @@ -467,9 +544,13 @@ ver = (PSF_PTPVER & VERSIONPTP_MASK) << VERSIONPTP_SHIFT; + mutex_lock(&clock->extreg_lock); + ext_write(0, phydev, PAGE5, PSF_CFG0, cfg0); ext_write(0, phydev, PAGE6, PSF_CFG1, ver); + mutex_unlock(&clock->extreg_lock); + if (!phydev->attached_dev) { pr_warn("expected to find an attached netdevice\n"); return; @@ -534,14 +615,18 @@ { s64 now, diff; struct phy_txts event_ts; - struct timespec ts; + struct timespec64 ts; struct list_head *this; struct dp83640_private *tmp; struct phy_device *master = clock->chosen->phydev; u16 cal_gpio, cfg0, evnt, ptp_trig, trigger, val; trigger = CAL_TRIGGER; - cal_gpio = gpio_tab[CALIBRATE_GPIO]; + cal_gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PHYSYNC, 0); + if (cal_gpio < 1) { + pr_err("PHY calibration pin not available - PHY is not calibrated."); + return; + } mutex_lock(&clock->extreg_lock); @@ -622,7 +707,7 @@ diff = now - (s64) phy2txts(&event_ts); pr_info("slave offset %lld nanoseconds\n", diff); diff += ADJTIME_FIX; - ts = ns_to_timespec(diff); + ts = ns_to_timespec64(diff); tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK); } @@ -646,7 +731,7 @@ } static int decode_evnt(struct dp83640_private *dp83640, - void *data, u16 ests) + void *data, int len, u16 ests) { struct phy_txts *phy_txts; struct ptp_clock_event event; @@ -654,6 +739,16 @@ int words = (ests >> EVNT_TS_LEN_SHIFT) & EVNT_TS_LEN_MASK; u16 ext_status = 0; + /* calculate length of the event timestamp status message */ + if (ests & MULT_EVNT) + parsed = (words + 2) * sizeof(u16); + else + parsed = (words + 1) * sizeof(u16); + + /* check if enough data is available */ + if (len < parsed) + return len; + if (ests & MULT_EVNT) { ext_status = *(u16 *) data; data += sizeof(ext_status); @@ -672,10 +767,7 @@ dp83640->edata.ns_lo = phy_txts->ns_lo; } - if (ext_status) { - parsed = words + 2; - } else { - parsed = words + 1; + if (!ext_status) { i = ((ests >> EVNT_NUM_SHIFT) & EVNT_NUM_MASK) - EXT_EVENT; ext_status = exts_chan_to_edata(i); } @@ -683,6 +775,9 @@ event.type = PTP_CLOCK_EXTTS; event.timestamp = phy2txts(&dp83640->edata); + /* Compensate for input path and synchronization delays */ + event.timestamp -= 35; + for (i = 0; i < N_EXT_TS; i++) { if (ext_status & exts_chan_to_edata(i)) { event.index = i; @@ -690,14 +785,71 @@ } } - return parsed * sizeof(u16); + return parsed; +} + +#define DP83640_PACKET_HASH_OFFSET 20 +#define DP83640_PACKET_HASH_LEN 10 + +static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts) +{ + u16 *seqid, hash; + unsigned int offset = 0; + u8 *msgtype, *data = skb_mac_header(skb); + + /* check sequenceID, messageType, 12 bit hash of offset 20-29 */ + + if (type & PTP_CLASS_VLAN) + offset += VLAN_HLEN; + + switch (type & PTP_CLASS_PMASK) { + case PTP_CLASS_IPV4: + offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; + break; + case PTP_CLASS_IPV6: + offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; + break; + case PTP_CLASS_L2: + offset += ETH_HLEN; + break; + default: + return 0; + } + + if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) + return 0; + + if (unlikely(type & PTP_CLASS_V1)) + msgtype = data + offset + OFF_PTP_CONTROL; + else + msgtype = data + offset; + if (rxts->msgtype != (*msgtype & 0xf)) + return 0; + + seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); + if (rxts->seqid != ntohs(*seqid)) + return 0; + + hash = ether_crc(DP83640_PACKET_HASH_LEN, + data + offset + DP83640_PACKET_HASH_OFFSET) >> 20; + if (rxts->hash != hash) + return 0; + + return 1; } static void decode_rxts(struct dp83640_private *dp83640, struct phy_rxts *phy_rxts) { struct rxts *rxts; + struct skb_shared_hwtstamps *shhwtstamps = NULL; + struct sk_buff *skb; unsigned long flags; + u8 overflow; + + overflow = (phy_rxts->ns_hi >> 14) & 0x3; + if (overflow) + pr_debug("rx timestamp queue overflow, count %d\n", overflow); spin_lock_irqsave(&dp83640->rx_lock, flags); @@ -710,7 +862,26 @@ rxts = list_first_entry(&dp83640->rxpool, struct rxts, list); list_del_init(&rxts->list); phy2rxts(phy_rxts, rxts); - list_add_tail(&rxts->list, &dp83640->rxts); + + spin_lock(&dp83640->rx_queue.lock); + skb_queue_walk(&dp83640->rx_queue, skb) { + struct dp83640_skb_info *skb_info; + + skb_info = (struct dp83640_skb_info *)skb->cb; + if (match(skb, skb_info->ptp_type, rxts)) { + __skb_unlink(skb, &dp83640->rx_queue); + shhwtstamps = skb_hwtstamps(skb); + memset(shhwtstamps, 0, sizeof(*shhwtstamps)); + shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns); + netif_rx_ni(skb); + list_add(&rxts->list, &dp83640->rxpool); + break; + } + } + spin_unlock(&dp83640->rx_queue.lock); + + if (!shhwtstamps) + list_add_tail(&rxts->list, &dp83640->rxts); out: spin_unlock_irqrestore(&dp83640->rx_lock, flags); } @@ -721,6 +892,7 @@ struct skb_shared_hwtstamps shhwtstamps; struct sk_buff *skb; u64 ns; + u8 overflow; /* We must already have the skb that triggered this. */ @@ -730,6 +902,17 @@ pr_debug("have timestamp but tx_queue empty\n"); return; } + + overflow = (phy_txts->ns_hi >> 14) & 0x3; + if (overflow) { + pr_debug("tx timestamp queue overflow, count %d\n", overflow); + while (skb) { + skb_complete_tx_timestamp(skb, NULL); + skb = skb_dequeue(&dp83640->tx_queue); + } + return; + } + ns = phy2txts(phy_txts); memset(&shhwtstamps, 0, sizeof(shhwtstamps)); shhwtstamps.hwtstamp = ns_to_ktime(ns); @@ -767,9 +950,9 @@ decode_txts(dp83640, phy_txts); size = sizeof(*phy_txts); - } else if (PSF_EVNT == type && len >= sizeof(*phy_txts)) { + } else if (PSF_EVNT == type) { - size = decode_evnt(dp83640, ptr, ests); + size = decode_evnt(dp83640, ptr, len, ests); } else { size = 0; @@ -784,20 +967,18 @@ u8 *data = skb->data, *msgtype; unsigned int offset = 0; - switch (type) { - case PTP_CLASS_V1_IPV4: - case PTP_CLASS_V2_IPV4: - offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; - break; - case PTP_CLASS_V1_IPV6: - case PTP_CLASS_V2_IPV6: - offset = OFF_PTP6; + if (type & PTP_CLASS_VLAN) + offset += VLAN_HLEN; + + switch (type & PTP_CLASS_PMASK) { + case PTP_CLASS_IPV4: + offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; break; - case PTP_CLASS_V2_L2: - offset = ETH_HLEN; + case PTP_CLASS_IPV6: + offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; break; - case PTP_CLASS_V2_VLAN: - offset = ETH_HLEN + VLAN_HLEN; + case PTP_CLASS_L2: + offset += ETH_HLEN; break; default: return 0; @@ -814,47 +995,6 @@ return (*msgtype & 0xf) == 0; } -static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts) -{ - u16 *seqid; - unsigned int offset; - u8 *msgtype, *data = skb_mac_header(skb); - - /* check sequenceID, messageType, 12 bit hash of offset 20-29 */ - - switch (type) { - case PTP_CLASS_V1_IPV4: - case PTP_CLASS_V2_IPV4: - offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; - break; - case PTP_CLASS_V1_IPV6: - case PTP_CLASS_V2_IPV6: - offset = OFF_PTP6; - break; - case PTP_CLASS_V2_L2: - offset = ETH_HLEN; - break; - case PTP_CLASS_V2_VLAN: - offset = ETH_HLEN + VLAN_HLEN; - break; - default: - return 0; - } - - if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) - return 0; - - if (unlikely(type & PTP_CLASS_V1)) - msgtype = data + offset + OFF_PTP_CONTROL; - else - msgtype = data + offset; - - seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); - - return (rxts->msgtype == (*msgtype & 0xf) && - rxts->seqid == ntohs(*seqid)); -} - static void dp83640_free_clocks(void) { struct dp83640_clock *clock; @@ -872,6 +1012,7 @@ mutex_destroy(&clock->extreg_lock); mutex_destroy(&clock->clock_lock); put_device(&clock->bus->dev); + kfree(clock->caps.pin_config); kfree(clock); } @@ -890,13 +1031,19 @@ clock->caps.max_adj = 1953124; clock->caps.n_alarm = 0; clock->caps.n_ext_ts = N_EXT_TS; - clock->caps.n_per_out = 1; + clock->caps.n_per_out = N_PER_OUT; + clock->caps.n_pins = DP83640_N_PINS; clock->caps.pps = 0; clock->caps.adjfreq = ptp_dp83640_adjfreq; clock->caps.adjtime = ptp_dp83640_adjtime; - clock->caps.gettime = ptp_dp83640_gettime; - clock->caps.settime = ptp_dp83640_settime; + clock->caps.gettime64 = ptp_dp83640_gettime; + clock->caps.settime64 = ptp_dp83640_settime; clock->caps.enable = ptp_dp83640_enable; + clock->caps.verify = ptp_dp83640_verify; + /* + * Convert the module param defaults into a dynamic pin configuration. + */ + dp83640_gpio_defaults(clock->caps.pin_config); /* * Get a reference to this bus instance. */ @@ -947,6 +1094,13 @@ if (!clock) goto out; + clock->caps.pin_config = kzalloc(sizeof(struct ptp_pin_desc) * + DP83640_N_PINS, GFP_KERNEL); + if (!clock->caps.pin_config) { + kfree(clock); + clock = NULL; + goto out; + } dp83640_clock_init(clock, bus); list_add_tail(&phyter_clocks, &clock->list); out: @@ -978,7 +1132,7 @@ goto no_memory; dp83640->phydev = phydev; - INIT_WORK(&dp83640->ts_work, rx_timestamp_work); + INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work); INIT_LIST_HEAD(&dp83640->rxts); INIT_LIST_HEAD(&dp83640->rxpool); @@ -1003,11 +1157,6 @@ } else list_add_tail(&dp83640->list, &clock->phylist); - if (clock->chosen && !list_empty(&clock->phylist)) - recalibrate(clock); - else - enable_broadcast(dp83640->phydev, clock->page, 1); - dp83640_clock_put(clock); return 0; @@ -1025,19 +1174,15 @@ struct dp83640_clock *clock; struct list_head *this, *next; struct dp83640_private *tmp, *dp83640 = phydev->priv; - struct sk_buff *skb; if (phydev->addr == BROADCAST_ADDR) return; enable_status_frames(phydev, false); - cancel_work_sync(&dp83640->ts_work); + cancel_delayed_work_sync(&dp83640->ts_work); - while ((skb = skb_dequeue(&dp83640->rx_queue)) != NULL) - kfree_skb(skb); - - while ((skb = skb_dequeue(&dp83640->tx_queue)) != NULL) - skb_complete_tx_timestamp(skb, NULL); + skb_queue_purge(&dp83640->rx_queue); + skb_queue_purge(&dp83640->tx_queue); clock = dp83640_clock_get(dp83640->clock); @@ -1058,6 +1203,28 @@ kfree(dp83640); } +static int dp83640_config_init(struct phy_device *phydev) +{ + struct dp83640_private *dp83640 = phydev->priv; + struct dp83640_clock *clock = dp83640->clock; + + if (clock->chosen && !list_empty(&clock->phylist)) + recalibrate(clock); + else { + mutex_lock(&clock->extreg_lock); + enable_broadcast(phydev, clock->page, 1); + mutex_unlock(&clock->extreg_lock); + } + + enable_status_frames(phydev, true); + + mutex_lock(&clock->extreg_lock); + ext_write(0, phydev, PAGE4, PTP_CTL, PTP_ENABLE); + mutex_unlock(&clock->extreg_lock); + + return 0; +} + static int dp83640_ack_interrupt(struct phy_device *phydev) { int err = phy_read(phydev, MII_DP83640_MISR); @@ -1144,29 +1311,29 @@ case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: dp83640->hwts_rx_en = 1; - dp83640->layer = LAYER4; - dp83640->version = 1; + dp83640->layer = PTP_CLASS_L4; + dp83640->version = PTP_CLASS_V1; break; case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: dp83640->hwts_rx_en = 1; - dp83640->layer = LAYER4; - dp83640->version = 2; + dp83640->layer = PTP_CLASS_L4; + dp83640->version = PTP_CLASS_V2; break; case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: dp83640->hwts_rx_en = 1; - dp83640->layer = LAYER2; - dp83640->version = 2; + dp83640->layer = PTP_CLASS_L2; + dp83640->version = PTP_CLASS_V2; break; case HWTSTAMP_FILTER_PTP_V2_EVENT: case HWTSTAMP_FILTER_PTP_V2_SYNC: case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: dp83640->hwts_rx_en = 1; - dp83640->layer = LAYER4|LAYER2; - dp83640->version = 2; + dp83640->layer = PTP_CLASS_L4 | PTP_CLASS_L2; + dp83640->version = PTP_CLASS_V2; break; default: return -ERANGE; @@ -1175,11 +1342,11 @@ txcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT; rxcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT; - if (dp83640->layer & LAYER2) { + if (dp83640->layer & PTP_CLASS_L2) { txcfg0 |= TX_L2_EN; rxcfg0 |= RX_L2_EN; } - if (dp83640->layer & LAYER4) { + if (dp83640->layer & PTP_CLASS_L4) { txcfg0 |= TX_IPV6_EN | TX_IPV4_EN; rxcfg0 |= RX_IPV6_EN | RX_IPV4_EN; } @@ -1195,11 +1362,6 @@ mutex_lock(&dp83640->clock->extreg_lock); - if (dp83640->hwts_tx_en || dp83640->hwts_rx_en) { - enable_status_frames(phydev, true); - ext_write(0, phydev, PAGE4, PTP_CTL, PTP_ENABLE); - } - ext_write(0, phydev, PAGE5, PTP_TXCFG0, txcfg0); ext_write(0, phydev, PAGE5, PTP_RXCFG0, rxcfg0); @@ -1211,48 +1373,35 @@ static void rx_timestamp_work(struct work_struct *work) { struct dp83640_private *dp83640 = - container_of(work, struct dp83640_private, ts_work); - struct list_head *this, *next; - struct rxts *rxts; - struct skb_shared_hwtstamps *shhwtstamps; + container_of(work, struct dp83640_private, ts_work.work); struct sk_buff *skb; - unsigned int type; - unsigned long flags; - - /* Deliver each deferred packet, with or without a time stamp. */ - while ((skb = skb_dequeue(&dp83640->rx_queue)) != NULL) { - type = SKB_PTP_TYPE(skb); - spin_lock_irqsave(&dp83640->rx_lock, flags); - list_for_each_safe(this, next, &dp83640->rxts) { - rxts = list_entry(this, struct rxts, list); - if (match(skb, type, rxts)) { - shhwtstamps = skb_hwtstamps(skb); - memset(shhwtstamps, 0, sizeof(*shhwtstamps)); - shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns); - list_del_init(&rxts->list); - list_add(&rxts->list, &dp83640->rxpool); - break; - } + /* Deliver expired packets. */ + while ((skb = skb_dequeue(&dp83640->rx_queue))) { + struct dp83640_skb_info *skb_info; + + skb_info = (struct dp83640_skb_info *)skb->cb; + if (!time_after(jiffies, skb_info->tmo)) { + skb_queue_head(&dp83640->rx_queue, skb); + break; } - spin_unlock_irqrestore(&dp83640->rx_lock, flags); + netif_rx_ni(skb); } - /* Clear out expired time stamps. */ - - spin_lock_irqsave(&dp83640->rx_lock, flags); - prune_rx_ts(dp83640); - spin_unlock_irqrestore(&dp83640->rx_lock, flags); + if (!skb_queue_empty(&dp83640->rx_queue)) + schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT); } static bool dp83640_rxtstamp(struct phy_device *phydev, struct sk_buff *skb, int type) { struct dp83640_private *dp83640 = phydev->priv; - - if (!dp83640->hwts_rx_en) - return false; + struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb; + struct list_head *this, *next; + struct rxts *rxts; + struct skb_shared_hwtstamps *shhwtstamps = NULL; + unsigned long flags; if (is_status_frame(skb, type)) { decode_status_frame(dp83640, skb); @@ -1260,9 +1409,36 @@ return true; } - SKB_PTP_TYPE(skb) = type; - skb_queue_tail(&dp83640->rx_queue, skb); - schedule_work(&dp83640->ts_work); + if (!dp83640->hwts_rx_en) + return false; + + if ((type & dp83640->version) == 0 || (type & dp83640->layer) == 0) + return false; + + spin_lock_irqsave(&dp83640->rx_lock, flags); + prune_rx_ts(dp83640); + list_for_each_safe(this, next, &dp83640->rxts) { + rxts = list_entry(this, struct rxts, list); + if (match(skb, type, rxts)) { + shhwtstamps = skb_hwtstamps(skb); + memset(shhwtstamps, 0, sizeof(*shhwtstamps)); + shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns); + netif_rx_ni(skb); + list_del_init(&rxts->list); + list_add(&rxts->list, &dp83640->rxpool); + break; + } + } + spin_unlock_irqrestore(&dp83640->rx_lock, flags); + + if (!shhwtstamps) { + skb_info->ptp_type = type; + skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT; + skb_queue_tail(&dp83640->rx_queue, skb); + schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT); + } else { + netif_rx_ni(skb); + } return true; } @@ -1276,18 +1452,18 @@ case HWTSTAMP_TX_ONESTEP_SYNC: if (is_sync(skb, type)) { - skb_complete_tx_timestamp(skb, NULL); + kfree_skb(skb); return; } /* fall through */ case HWTSTAMP_TX_ON: + skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; skb_queue_tail(&dp83640->tx_queue, skb); - schedule_work(&dp83640->ts_work); break; case HWTSTAMP_TX_OFF: default: - skb_complete_tx_timestamp(skb, NULL); + kfree_skb(skb); break; } } @@ -1308,17 +1484,9 @@ info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) | (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) | - (1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) | - (1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) | (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | - (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | - (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | - (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | - (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | - (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | - (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | - (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ); + (1 << HWTSTAMP_FILTER_PTP_V2_EVENT); return 0; } @@ -1330,6 +1498,7 @@ .flags = PHY_HAS_INTERRUPT, .probe = dp83640_probe, .remove = dp83640_remove, + .config_init = dp83640_config_init, .config_aneg = genphy_config_aneg, .read_status = genphy_read_status, .ack_interrupt = dp83640_ack_interrupt, @@ -1353,7 +1522,7 @@ } MODULE_DESCRIPTION("National Semiconductor DP83640 PHY driver"); -MODULE_AUTHOR("Richard Cochran "); +MODULE_AUTHOR("Richard Cochran "); MODULE_LICENSE("GPL"); module_init(dp83640_init);