From bedbef8ad3e75a304af6361af235302bcc61d06b Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Tue, 14 May 2024 06:39:01 +0000
Subject: [PATCH] 修改内核路径
---
kernel/drivers/net/phy/dp83640.c | 352 +++++++++++++++++++++++++++-------------------------------
1 files changed, 164 insertions(+), 188 deletions(-)
diff --git a/kernel/drivers/net/phy/dp83640.c b/kernel/drivers/net/phy/dp83640.c
index dd0a658..f2cacca 100644
--- a/kernel/drivers/net/phy/dp83640.c
+++ b/kernel/drivers/net/phy/dp83640.c
@@ -1,21 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0+
/*
* Driver for the National Semiconductor DP83640 PHYTER
*
* Copyright (C) 2010 OMICRON electronics GmbH
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -111,6 +98,7 @@
struct list_head list;
struct dp83640_clock *clock;
struct phy_device *phydev;
+ struct mii_timestamper mii_ts;
struct delayed_work ts_work;
int hwts_tx_en;
int hwts_rx_en;
@@ -482,6 +470,19 @@
switch (rq->type) {
case PTP_CLK_REQ_EXTTS:
+ /* Reject requests with unsupported flags */
+ if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
+ PTP_RISING_EDGE |
+ PTP_FALLING_EDGE |
+ PTP_STRICT_FLAGS))
+ return -EOPNOTSUPP;
+
+ /* Reject requests to enable time stamping on both edges. */
+ if ((rq->extts.flags & PTP_STRICT_FLAGS) &&
+ (rq->extts.flags & PTP_ENABLE_FEATURE) &&
+ (rq->extts.flags & PTP_EXTTS_EDGES) == PTP_EXTTS_EDGES)
+ return -EOPNOTSUPP;
+
index = rq->extts.index;
if (index >= N_EXT_TS)
return -EINVAL;
@@ -504,6 +505,9 @@
return 0;
case PTP_CLK_REQ_PEROUT:
+ /* Reject requests with unsupported flags */
+ if (rq->perout.flags)
+ return -EOPNOTSUPP;
if (rq->perout.index >= N_PER_OUT)
return -EINVAL;
return periodic_output(clock, rq, on, rq->perout.index);
@@ -553,16 +557,17 @@
mutex_unlock(&clock->extreg_lock);
if (!phydev->attached_dev) {
- pr_warn("expected to find an attached netdevice\n");
+ phydev_warn(phydev,
+ "expected to find an attached netdevice\n");
return;
}
if (on) {
if (dev_mc_add(phydev->attached_dev, status_frame_dst))
- pr_warn("failed to add mc address\n");
+ phydev_warn(phydev, "failed to add mc address\n");
} else {
if (dev_mc_del(phydev->attached_dev, status_frame_dst))
- pr_warn("failed to delete mc address\n");
+ phydev_warn(phydev, "failed to delete mc address\n");
}
}
@@ -623,7 +628,7 @@
u16 cal_gpio, cfg0, evnt, ptp_trig, trigger, val;
trigger = CAL_TRIGGER;
- cal_gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
+ cal_gpio = 1 + ptp_find_pin_unlocked(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
if (cal_gpio < 1) {
pr_err("PHY calibration pin not available - PHY is not calibrated.");
return;
@@ -686,9 +691,9 @@
* read out and correct offsets
*/
val = ext_read(master, PAGE4, PTP_STS);
- pr_info("master PTP_STS 0x%04hx\n", val);
+ phydev_info(master, "master PTP_STS 0x%04hx\n", val);
val = ext_read(master, PAGE4, PTP_ESTS);
- pr_info("master PTP_ESTS 0x%04hx\n", val);
+ phydev_info(master, "master PTP_ESTS 0x%04hx\n", val);
event_ts.ns_lo = ext_read(master, PAGE4, PTP_EDATA);
event_ts.ns_hi = ext_read(master, PAGE4, PTP_EDATA);
event_ts.sec_lo = ext_read(master, PAGE4, PTP_EDATA);
@@ -698,15 +703,16 @@
list_for_each(this, &clock->phylist) {
tmp = list_entry(this, struct dp83640_private, list);
val = ext_read(tmp->phydev, PAGE4, PTP_STS);
- pr_info("slave PTP_STS 0x%04hx\n", val);
+ phydev_info(tmp->phydev, "slave PTP_STS 0x%04hx\n", val);
val = ext_read(tmp->phydev, PAGE4, PTP_ESTS);
- pr_info("slave PTP_ESTS 0x%04hx\n", val);
+ phydev_info(tmp->phydev, "slave PTP_ESTS 0x%04hx\n", val);
event_ts.ns_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
event_ts.ns_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
event_ts.sec_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
event_ts.sec_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
diff = now - (s64) phy2txts(&event_ts);
- pr_info("slave offset %lld nanoseconds\n", diff);
+ phydev_info(tmp->phydev, "slave offset %lld nanoseconds\n",
+ diff);
diff += ADJTIME_FIX;
ts = ns_to_timespec64(diff);
tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK);
@@ -760,13 +766,13 @@
switch (words) {
case 3:
dp83640->edata.sec_hi = phy_txts->sec_hi;
- /* fall through */
+ fallthrough;
case 2:
dp83640->edata.sec_lo = phy_txts->sec_lo;
- /* fall through */
+ fallthrough;
case 1:
dp83640->edata.ns_hi = phy_txts->ns_hi;
- /* fall through */
+ fallthrough;
case 0:
dp83640->edata.ns_lo = phy_txts->ns_lo;
}
@@ -792,50 +798,32 @@
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);
+ struct ptp_header *hdr;
+ u8 msgtype;
+ u16 seqid;
+ u16 hash;
/* 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))
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
return 0;
- if (unlikely(type & PTP_CLASS_V1))
- msgtype = data + offset + OFF_PTP_CONTROL;
- else
- msgtype = data + offset;
- if (rxts->msgtype != (*msgtype & 0xf))
+ msgtype = ptp_get_msgtype(hdr, type);
+
+ if (rxts->msgtype != (msgtype & 0xf))
return 0;
- seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
- if (rxts->seqid != ntohs(*seqid))
+ seqid = be16_to_cpu(hdr->sequence_id);
+ if (rxts->seqid != seqid)
return 0;
hash = ether_crc(DP83640_PACKET_HASH_LEN,
- data + offset + DP83640_PACKET_HASH_OFFSET) >> 20;
+ (unsigned char *)&hdr->source_port_identity) >> 20;
if (rxts->hash != hash)
return 0;
@@ -975,35 +963,16 @@
static int is_sync(struct sk_buff *skb, int type)
{
- u8 *data = skb->data, *msgtype;
- unsigned int offset = 0;
+ struct ptp_header *hdr;
+ u8 msgtype;
- 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 (type & PTP_CLASS_V1)
- offset += OFF_PTP_CONTROL;
-
- if (skb->len < offset + 1)
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
return 0;
- msgtype = data + offset;
+ msgtype = ptp_get_msgtype(hdr, type);
- return (*msgtype & 0xf) == 0;
+ return (msgtype & 0xf) == 0;
}
static void dp83640_free_clocks(void)
@@ -1126,96 +1095,6 @@
mutex_unlock(&clock->clock_lock);
}
-static int dp83640_probe(struct phy_device *phydev)
-{
- struct dp83640_clock *clock;
- struct dp83640_private *dp83640;
- int err = -ENOMEM, i;
-
- if (phydev->mdio.addr == BROADCAST_ADDR)
- return 0;
-
- clock = dp83640_clock_get_bus(phydev->mdio.bus);
- if (!clock)
- goto no_clock;
-
- dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
- if (!dp83640)
- goto no_memory;
-
- dp83640->phydev = phydev;
- INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
-
- INIT_LIST_HEAD(&dp83640->rxts);
- INIT_LIST_HEAD(&dp83640->rxpool);
- for (i = 0; i < MAX_RXTS; i++)
- list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
-
- phydev->priv = dp83640;
-
- spin_lock_init(&dp83640->rx_lock);
- skb_queue_head_init(&dp83640->rx_queue);
- skb_queue_head_init(&dp83640->tx_queue);
-
- dp83640->clock = clock;
-
- if (choose_this_phy(clock, phydev)) {
- clock->chosen = dp83640;
- clock->ptp_clock = ptp_clock_register(&clock->caps,
- &phydev->mdio.dev);
- if (IS_ERR(clock->ptp_clock)) {
- err = PTR_ERR(clock->ptp_clock);
- goto no_register;
- }
- } else
- list_add_tail(&dp83640->list, &clock->phylist);
-
- dp83640_clock_put(clock);
- return 0;
-
-no_register:
- clock->chosen = NULL;
- kfree(dp83640);
-no_memory:
- dp83640_clock_put(clock);
-no_clock:
- return err;
-}
-
-static void dp83640_remove(struct phy_device *phydev)
-{
- struct dp83640_clock *clock;
- struct list_head *this, *next;
- struct dp83640_private *tmp, *dp83640 = phydev->priv;
-
- if (phydev->mdio.addr == BROADCAST_ADDR)
- return;
-
- enable_status_frames(phydev, false);
- cancel_delayed_work_sync(&dp83640->ts_work);
-
- skb_queue_purge(&dp83640->rx_queue);
- skb_queue_purge(&dp83640->tx_queue);
-
- clock = dp83640_clock_get(dp83640->clock);
-
- if (dp83640 == clock->chosen) {
- ptp_clock_unregister(clock->ptp_clock);
- clock->chosen = NULL;
- } else {
- list_for_each_safe(this, next, &clock->phylist) {
- tmp = list_entry(this, struct dp83640_private, list);
- if (tmp == dp83640) {
- list_del_init(&tmp->list);
- break;
- }
- }
- }
-
- dp83640_clock_put(clock);
- kfree(dp83640);
-}
-
static int dp83640_soft_reset(struct phy_device *phydev)
{
int ret;
@@ -1314,9 +1193,10 @@
}
}
-static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
+static int dp83640_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
{
- struct dp83640_private *dp83640 = phydev->priv;
+ struct dp83640_private *dp83640 =
+ container_of(mii_ts, struct dp83640_private, mii_ts);
struct hwtstamp_config cfg;
u16 txcfg0, rxcfg0;
@@ -1396,8 +1276,8 @@
mutex_lock(&dp83640->clock->extreg_lock);
- ext_write(0, phydev, PAGE5, PTP_TXCFG0, txcfg0);
- ext_write(0, phydev, PAGE5, PTP_RXCFG0, rxcfg0);
+ ext_write(0, dp83640->phydev, PAGE5, PTP_TXCFG0, txcfg0);
+ ext_write(0, dp83640->phydev, PAGE5, PTP_RXCFG0, rxcfg0);
mutex_unlock(&dp83640->clock->extreg_lock);
@@ -1427,10 +1307,11 @@
schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
}
-static bool dp83640_rxtstamp(struct phy_device *phydev,
+static bool dp83640_rxtstamp(struct mii_timestamper *mii_ts,
struct sk_buff *skb, int type)
{
- struct dp83640_private *dp83640 = phydev->priv;
+ struct dp83640_private *dp83640 =
+ container_of(mii_ts, struct dp83640_private, mii_ts);
struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
struct list_head *this, *next;
struct rxts *rxts;
@@ -1476,11 +1357,12 @@
return true;
}
-static void dp83640_txtstamp(struct phy_device *phydev,
+static void dp83640_txtstamp(struct mii_timestamper *mii_ts,
struct sk_buff *skb, int type)
{
struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
- struct dp83640_private *dp83640 = phydev->priv;
+ struct dp83640_private *dp83640 =
+ container_of(mii_ts, struct dp83640_private, mii_ts);
switch (dp83640->hwts_tx_en) {
@@ -1489,7 +1371,7 @@
kfree_skb(skb);
return;
}
- /* fall through */
+ fallthrough;
case HWTSTAMP_TX_ON:
skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
@@ -1503,9 +1385,11 @@
}
}
-static int dp83640_ts_info(struct phy_device *dev, struct ethtool_ts_info *info)
+static int dp83640_ts_info(struct mii_timestamper *mii_ts,
+ struct ethtool_ts_info *info)
{
- struct dp83640_private *dp83640 = dev->priv;
+ struct dp83640_private *dp83640 =
+ container_of(mii_ts, struct dp83640_private, mii_ts);
info->so_timestamping =
SOF_TIMESTAMPING_TX_HARDWARE |
@@ -1525,22 +1409,114 @@
return 0;
}
+static int dp83640_probe(struct phy_device *phydev)
+{
+ struct dp83640_clock *clock;
+ struct dp83640_private *dp83640;
+ int err = -ENOMEM, i;
+
+ if (phydev->mdio.addr == BROADCAST_ADDR)
+ return 0;
+
+ clock = dp83640_clock_get_bus(phydev->mdio.bus);
+ if (!clock)
+ goto no_clock;
+
+ dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
+ if (!dp83640)
+ goto no_memory;
+
+ dp83640->phydev = phydev;
+ dp83640->mii_ts.rxtstamp = dp83640_rxtstamp;
+ dp83640->mii_ts.txtstamp = dp83640_txtstamp;
+ dp83640->mii_ts.hwtstamp = dp83640_hwtstamp;
+ dp83640->mii_ts.ts_info = dp83640_ts_info;
+
+ INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
+ INIT_LIST_HEAD(&dp83640->rxts);
+ INIT_LIST_HEAD(&dp83640->rxpool);
+ for (i = 0; i < MAX_RXTS; i++)
+ list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
+
+ phydev->mii_ts = &dp83640->mii_ts;
+ phydev->priv = dp83640;
+
+ spin_lock_init(&dp83640->rx_lock);
+ skb_queue_head_init(&dp83640->rx_queue);
+ skb_queue_head_init(&dp83640->tx_queue);
+
+ dp83640->clock = clock;
+
+ if (choose_this_phy(clock, phydev)) {
+ clock->chosen = dp83640;
+ clock->ptp_clock = ptp_clock_register(&clock->caps,
+ &phydev->mdio.dev);
+ if (IS_ERR(clock->ptp_clock)) {
+ err = PTR_ERR(clock->ptp_clock);
+ goto no_register;
+ }
+ } else
+ list_add_tail(&dp83640->list, &clock->phylist);
+
+ dp83640_clock_put(clock);
+ return 0;
+
+no_register:
+ clock->chosen = NULL;
+ kfree(dp83640);
+no_memory:
+ dp83640_clock_put(clock);
+no_clock:
+ return err;
+}
+
+static void dp83640_remove(struct phy_device *phydev)
+{
+ struct dp83640_clock *clock;
+ struct list_head *this, *next;
+ struct dp83640_private *tmp, *dp83640 = phydev->priv;
+
+ if (phydev->mdio.addr == BROADCAST_ADDR)
+ return;
+
+ phydev->mii_ts = NULL;
+
+ enable_status_frames(phydev, false);
+ cancel_delayed_work_sync(&dp83640->ts_work);
+
+ skb_queue_purge(&dp83640->rx_queue);
+ skb_queue_purge(&dp83640->tx_queue);
+
+ clock = dp83640_clock_get(dp83640->clock);
+
+ if (dp83640 == clock->chosen) {
+ ptp_clock_unregister(clock->ptp_clock);
+ clock->chosen = NULL;
+ } else {
+ list_for_each_safe(this, next, &clock->phylist) {
+ tmp = list_entry(this, struct dp83640_private, list);
+ if (tmp == dp83640) {
+ list_del_init(&tmp->list);
+ break;
+ }
+ }
+ }
+
+ dp83640_clock_put(clock);
+ kfree(dp83640);
+}
+
static struct phy_driver dp83640_driver = {
.phy_id = DP83640_PHY_ID,
.phy_id_mask = 0xfffffff0,
.name = "NatSemi DP83640",
- .features = PHY_BASIC_FEATURES,
- .flags = PHY_HAS_INTERRUPT,
+ /* PHY_BASIC_FEATURES */
.probe = dp83640_probe,
.remove = dp83640_remove,
.soft_reset = dp83640_soft_reset,
.config_init = dp83640_config_init,
.ack_interrupt = dp83640_ack_interrupt,
.config_intr = dp83640_config_intr,
- .ts_info = dp83640_ts_info,
- .hwtstamp = dp83640_hwtstamp,
- .rxtstamp = dp83640_rxtstamp,
- .txtstamp = dp83640_txtstamp,
};
static int __init dp83640_init(void)
--
Gitblit v1.6.2