forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-01-05 071106ecf68c401173c58808b1cf5f68cc50d390 
 kernel/drivers/net/phy/dp83640.c
....@@ -1,21 +1,8 @@
1
+// SPDX-License-Identifier: GPL-2.0+
12 /*
23  * Driver for the National Semiconductor DP83640 PHYTER
34  *
45  * 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.
196  */
207 
218 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
....@@ -111,6 +98,7 @@
11198 	struct list_head list;
11299 	struct dp83640_clock *clock;
113100 	struct phy_device *phydev;
101
+	struct mii_timestamper mii_ts;
114102 	struct delayed_work ts_work;
115103 	int hwts_tx_en;
116104 	int hwts_rx_en;
....@@ -482,6 +470,19 @@
482470 
483471 	switch (rq->type) {
484472 	case PTP_CLK_REQ_EXTTS:
473
+		/* Reject requests with unsupported flags */
474
+		if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
475
+					PTP_RISING_EDGE |
476
+					PTP_FALLING_EDGE |
477
+					PTP_STRICT_FLAGS))
478
+			return -EOPNOTSUPP;
479
+
480
+		/* Reject requests to enable time stamping on both edges. */
481
+		if ((rq->extts.flags & PTP_STRICT_FLAGS) &&
482
+		    (rq->extts.flags & PTP_ENABLE_FEATURE) &&
483
+		    (rq->extts.flags & PTP_EXTTS_EDGES) == PTP_EXTTS_EDGES)
484
+			return -EOPNOTSUPP;
485
+
485486 		index = rq->extts.index;
486487 		if (index >= N_EXT_TS)
487488 			return -EINVAL;
....@@ -504,6 +505,9 @@
504505 		return 0;
505506 
506507 	case PTP_CLK_REQ_PEROUT:
508
+		/* Reject requests with unsupported flags */
509
+		if (rq->perout.flags)
510
+			return -EOPNOTSUPP;
507511 		if (rq->perout.index >= N_PER_OUT)
508512 			return -EINVAL;
509513 		return periodic_output(clock, rq, on, rq->perout.index);
....@@ -553,16 +557,17 @@
553557 	mutex_unlock(&clock->extreg_lock);
554558 
555559 	if (!phydev->attached_dev) {
556
-		pr_warn("expected to find an attached netdevice\n");
560
+		phydev_warn(phydev,
561
+			    "expected to find an attached netdevice\n");
557562 		return;
558563 	}
559564 
560565 	if (on) {
561566 		if (dev_mc_add(phydev->attached_dev, status_frame_dst))
562
-			pr_warn("failed to add mc address\n");
567
+			phydev_warn(phydev, "failed to add mc address\n");
563568 	} else {
564569 		if (dev_mc_del(phydev->attached_dev, status_frame_dst))
565
-			pr_warn("failed to delete mc address\n");
570
+			phydev_warn(phydev, "failed to delete mc address\n");
566571 	}
567572 }
568573 
....@@ -623,7 +628,7 @@
623628 	u16 cal_gpio, cfg0, evnt, ptp_trig, trigger, val;
624629 
625630 	trigger = CAL_TRIGGER;
626
-	cal_gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
631
+	cal_gpio = 1 + ptp_find_pin_unlocked(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
627632 	if (cal_gpio < 1) {
628633 		pr_err("PHY calibration pin not available - PHY is not calibrated.");
629634 		return;
....@@ -686,9 +691,9 @@
686691 	 * read out and correct offsets
687692 	 */
688693 	val = ext_read(master, PAGE4, PTP_STS);
689
-	pr_info("master PTP_STS  0x%04hx\n", val);
694
+	phydev_info(master, "master PTP_STS  0x%04hx\n", val);
690695 	val = ext_read(master, PAGE4, PTP_ESTS);
691
-	pr_info("master PTP_ESTS 0x%04hx\n", val);
696
+	phydev_info(master, "master PTP_ESTS 0x%04hx\n", val);
692697 	event_ts.ns_lo  = ext_read(master, PAGE4, PTP_EDATA);
693698 	event_ts.ns_hi  = ext_read(master, PAGE4, PTP_EDATA);
694699 	event_ts.sec_lo = ext_read(master, PAGE4, PTP_EDATA);
....@@ -698,15 +703,16 @@
698703 	list_for_each(this, &clock->phylist) {
699704 		tmp = list_entry(this, struct dp83640_private, list);
700705 		val = ext_read(tmp->phydev, PAGE4, PTP_STS);
701
-		pr_info("slave  PTP_STS  0x%04hx\n", val);
706
+		phydev_info(tmp->phydev, "slave  PTP_STS  0x%04hx\n", val);
702707 		val = ext_read(tmp->phydev, PAGE4, PTP_ESTS);
703
-		pr_info("slave  PTP_ESTS 0x%04hx\n", val);
708
+		phydev_info(tmp->phydev, "slave  PTP_ESTS 0x%04hx\n", val);
704709 		event_ts.ns_lo  = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
705710 		event_ts.ns_hi  = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
706711 		event_ts.sec_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
707712 		event_ts.sec_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
708713 		diff = now - (s64) phy2txts(&event_ts);
709
-		pr_info("slave offset %lld nanoseconds\n", diff);
714
+		phydev_info(tmp->phydev, "slave offset %lld nanoseconds\n",
715
+			    diff);
710716 		diff += ADJTIME_FIX;
711717 		ts = ns_to_timespec64(diff);
712718 		tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK);
....@@ -760,13 +766,13 @@
760766 	switch (words) {
761767 	case 3:
762768 		dp83640->edata.sec_hi = phy_txts->sec_hi;
763
-		/* fall through */
769
+		fallthrough;
764770 	case 2:
765771 		dp83640->edata.sec_lo = phy_txts->sec_lo;
766
-		/* fall through */
772
+		fallthrough;
767773 	case 1:
768774 		dp83640->edata.ns_hi = phy_txts->ns_hi;
769
-		/* fall through */
775
+		fallthrough;
770776 	case 0:
771777 		dp83640->edata.ns_lo = phy_txts->ns_lo;
772778 	}
....@@ -792,50 +798,32 @@
792798 	return parsed;
793799 }
794800 
795
-#define DP83640_PACKET_HASH_OFFSET	20
796801 #define DP83640_PACKET_HASH_LEN		10
797802 
798803 static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
799804 {
800
-	u16 *seqid, hash;
801
-	unsigned int offset = 0;
802
-	u8 *msgtype, *data = skb_mac_header(skb);
805
+	struct ptp_header *hdr;
806
+	u8 msgtype;
807
+	u16 seqid;
808
+	u16 hash;
803809 
804810 	/* check sequenceID, messageType, 12 bit hash of offset 20-29 */
805811 
806
-	if (type & PTP_CLASS_VLAN)
807
-		offset += VLAN_HLEN;
808
-
809
-	switch (type & PTP_CLASS_PMASK) {
810
-	case PTP_CLASS_IPV4:
811
-		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
812
-		break;
813
-	case PTP_CLASS_IPV6:
814
-		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
815
-		break;
816
-	case PTP_CLASS_L2:
817
-		offset += ETH_HLEN;
818
-		break;
819
-	default:
820
-		return 0;
821
-	}
822
-
823
-	if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
812
+	hdr = ptp_parse_header(skb, type);
813
+	if (!hdr)
824814 		return 0;
825815 
826
-	if (unlikely(type & PTP_CLASS_V1))
827
-		msgtype = data + offset + OFF_PTP_CONTROL;
828
-	else
829
-		msgtype = data + offset;
830
-	if (rxts->msgtype != (*msgtype & 0xf))
816
+	msgtype = ptp_get_msgtype(hdr, type);
817
+
818
+	if (rxts->msgtype != (msgtype & 0xf))
831819 		return 0;
832820 
833
-	seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
834
-	if (rxts->seqid != ntohs(*seqid))
821
+	seqid = be16_to_cpu(hdr->sequence_id);
822
+	if (rxts->seqid != seqid)
835823 		return 0;
836824 
837825 	hash = ether_crc(DP83640_PACKET_HASH_LEN,
838
-			 data + offset + DP83640_PACKET_HASH_OFFSET) >> 20;
826
+			 (unsigned char *)&hdr->source_port_identity) >> 20;
839827 	if (rxts->hash != hash)
840828 		return 0;
841829 
....@@ -975,35 +963,16 @@
975963 
976964 static int is_sync(struct sk_buff *skb, int type)
977965 {
978
-	u8 *data = skb->data, *msgtype;
979
-	unsigned int offset = 0;
966
+	struct ptp_header *hdr;
967
+	u8 msgtype;
980968 
981
-	if (type & PTP_CLASS_VLAN)
982
-		offset += VLAN_HLEN;
983
-
984
-	switch (type & PTP_CLASS_PMASK) {
985
-	case PTP_CLASS_IPV4:
986
-		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
987
-		break;
988
-	case PTP_CLASS_IPV6:
989
-		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
990
-		break;
991
-	case PTP_CLASS_L2:
992
-		offset += ETH_HLEN;
993
-		break;
994
-	default:
995
-		return 0;
996
-	}
997
-
998
-	if (type & PTP_CLASS_V1)
999
-		offset += OFF_PTP_CONTROL;
1000
-
1001
-	if (skb->len < offset + 1)
969
+	hdr = ptp_parse_header(skb, type);
970
+	if (!hdr)
1002971 		return 0;
1003972 
1004
-	msgtype = data + offset;
973
+	msgtype = ptp_get_msgtype(hdr, type);
1005974 
1006
-	return (*msgtype & 0xf) == 0;
975
+	return (msgtype & 0xf) == 0;
1007976 }
1008977 
1009978 static void dp83640_free_clocks(void)
....@@ -1126,96 +1095,6 @@
11261095 	mutex_unlock(&clock->clock_lock);
11271096 }
11281097 
1129
-static int dp83640_probe(struct phy_device *phydev)
1130
-{
1131
-	struct dp83640_clock *clock;
1132
-	struct dp83640_private *dp83640;
1133
-	int err = -ENOMEM, i;
1134
-
1135
-	if (phydev->mdio.addr == BROADCAST_ADDR)
1136
-		return 0;
1137
-
1138
-	clock = dp83640_clock_get_bus(phydev->mdio.bus);
1139
-	if (!clock)
1140
-		goto no_clock;
1141
-
1142
-	dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
1143
-	if (!dp83640)
1144
-		goto no_memory;
1145
-
1146
-	dp83640->phydev = phydev;
1147
-	INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
1148
-
1149
-	INIT_LIST_HEAD(&dp83640->rxts);
1150
-	INIT_LIST_HEAD(&dp83640->rxpool);
1151
-	for (i = 0; i < MAX_RXTS; i++)
1152
-		list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
1153
-
1154
-	phydev->priv = dp83640;
1155
-
1156
-	spin_lock_init(&dp83640->rx_lock);
1157
-	skb_queue_head_init(&dp83640->rx_queue);
1158
-	skb_queue_head_init(&dp83640->tx_queue);
1159
-
1160
-	dp83640->clock = clock;
1161
-
1162
-	if (choose_this_phy(clock, phydev)) {
1163
-		clock->chosen = dp83640;
1164
-		clock->ptp_clock = ptp_clock_register(&clock->caps,
1165
-						      &phydev->mdio.dev);
1166
-		if (IS_ERR(clock->ptp_clock)) {
1167
-			err = PTR_ERR(clock->ptp_clock);
1168
-			goto no_register;
1169
-		}
1170
-	} else
1171
-		list_add_tail(&dp83640->list, &clock->phylist);
1172
-
1173
-	dp83640_clock_put(clock);
1174
-	return 0;
1175
-
1176
-no_register:
1177
-	clock->chosen = NULL;
1178
-	kfree(dp83640);
1179
-no_memory:
1180
-	dp83640_clock_put(clock);
1181
-no_clock:
1182
-	return err;
1183
-}
1184
-
1185
-static void dp83640_remove(struct phy_device *phydev)
1186
-{
1187
-	struct dp83640_clock *clock;
1188
-	struct list_head *this, *next;
1189
-	struct dp83640_private *tmp, *dp83640 = phydev->priv;
1190
-
1191
-	if (phydev->mdio.addr == BROADCAST_ADDR)
1192
-		return;
1193
-
1194
-	enable_status_frames(phydev, false);
1195
-	cancel_delayed_work_sync(&dp83640->ts_work);
1196
-
1197
-	skb_queue_purge(&dp83640->rx_queue);
1198
-	skb_queue_purge(&dp83640->tx_queue);
1199
-
1200
-	clock = dp83640_clock_get(dp83640->clock);
1201
-
1202
-	if (dp83640 == clock->chosen) {
1203
-		ptp_clock_unregister(clock->ptp_clock);
1204
-		clock->chosen = NULL;
1205
-	} else {
1206
-		list_for_each_safe(this, next, &clock->phylist) {
1207
-			tmp = list_entry(this, struct dp83640_private, list);
1208
-			if (tmp == dp83640) {
1209
-				list_del_init(&tmp->list);
1210
-				break;
1211
-			}
1212
-		}
1213
-	}
1214
-
1215
-	dp83640_clock_put(clock);
1216
-	kfree(dp83640);
1217
-}
1218
-
12191098 static int dp83640_soft_reset(struct phy_device *phydev)
12201099 {
12211100 	int ret;
....@@ -1314,9 +1193,10 @@
13141193 	}
13151194 }
13161195 
1317
-static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
1196
+static int dp83640_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
13181197 {
1319
-	struct dp83640_private *dp83640 = phydev->priv;
1198
+	struct dp83640_private *dp83640 =
1199
+		container_of(mii_ts, struct dp83640_private, mii_ts);
13201200 	struct hwtstamp_config cfg;
13211201 	u16 txcfg0, rxcfg0;
13221202 
....@@ -1396,8 +1276,8 @@
13961276 
13971277 	mutex_lock(&dp83640->clock->extreg_lock);
13981278 
1399
-	ext_write(0, phydev, PAGE5, PTP_TXCFG0, txcfg0);
1400
-	ext_write(0, phydev, PAGE5, PTP_RXCFG0, rxcfg0);
1279
+	ext_write(0, dp83640->phydev, PAGE5, PTP_TXCFG0, txcfg0);
1280
+	ext_write(0, dp83640->phydev, PAGE5, PTP_RXCFG0, rxcfg0);
14011281 
14021282 	mutex_unlock(&dp83640->clock->extreg_lock);
14031283 
....@@ -1427,10 +1307,11 @@
14271307 		schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
14281308 }
14291309 
1430
-static bool dp83640_rxtstamp(struct phy_device *phydev,
1310
+static bool dp83640_rxtstamp(struct mii_timestamper *mii_ts,
14311311 			     struct sk_buff *skb, int type)
14321312 {
1433
-	struct dp83640_private *dp83640 = phydev->priv;
1313
+	struct dp83640_private *dp83640 =
1314
+		container_of(mii_ts, struct dp83640_private, mii_ts);
14341315 	struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
14351316 	struct list_head *this, *next;
14361317 	struct rxts *rxts;
....@@ -1476,11 +1357,12 @@
14761357 	return true;
14771358 }
14781359 
1479
-static void dp83640_txtstamp(struct phy_device *phydev,
1360
+static void dp83640_txtstamp(struct mii_timestamper *mii_ts,
14801361 			     struct sk_buff *skb, int type)
14811362 {
14821363 	struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
1483
-	struct dp83640_private *dp83640 = phydev->priv;
1364
+	struct dp83640_private *dp83640 =
1365
+		container_of(mii_ts, struct dp83640_private, mii_ts);
14841366 
14851367 	switch (dp83640->hwts_tx_en) {
14861368 
....@@ -1489,7 +1371,7 @@
14891371 			kfree_skb(skb);
14901372 			return;
14911373 		}
1492
-		/* fall through */
1374
+		fallthrough;
14931375 	case HWTSTAMP_TX_ON:
14941376 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
14951377 		skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
....@@ -1503,9 +1385,11 @@
15031385 	}
15041386 }
15051387 
1506
-static int dp83640_ts_info(struct phy_device *dev, struct ethtool_ts_info *info)
1388
+static int dp83640_ts_info(struct mii_timestamper *mii_ts,
1389
+			   struct ethtool_ts_info *info)
15071390 {
1508
-	struct dp83640_private *dp83640 = dev->priv;
1391
+	struct dp83640_private *dp83640 =
1392
+		container_of(mii_ts, struct dp83640_private, mii_ts);
15091393 
15101394 	info->so_timestamping =
15111395 		SOF_TIMESTAMPING_TX_HARDWARE |
....@@ -1525,22 +1409,114 @@
15251409 	return 0;
15261410 }
15271411 
1412
+static int dp83640_probe(struct phy_device *phydev)
1413
+{
1414
+	struct dp83640_clock *clock;
1415
+	struct dp83640_private *dp83640;
1416
+	int err = -ENOMEM, i;
1417
+
1418
+	if (phydev->mdio.addr == BROADCAST_ADDR)
1419
+		return 0;
1420
+
1421
+	clock = dp83640_clock_get_bus(phydev->mdio.bus);
1422
+	if (!clock)
1423
+		goto no_clock;
1424
+
1425
+	dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
1426
+	if (!dp83640)
1427
+		goto no_memory;
1428
+
1429
+	dp83640->phydev = phydev;
1430
+	dp83640->mii_ts.rxtstamp = dp83640_rxtstamp;
1431
+	dp83640->mii_ts.txtstamp = dp83640_txtstamp;
1432
+	dp83640->mii_ts.hwtstamp = dp83640_hwtstamp;
1433
+	dp83640->mii_ts.ts_info  = dp83640_ts_info;
1434
+
1435
+	INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
1436
+	INIT_LIST_HEAD(&dp83640->rxts);
1437
+	INIT_LIST_HEAD(&dp83640->rxpool);
1438
+	for (i = 0; i < MAX_RXTS; i++)
1439
+		list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
1440
+
1441
+	phydev->mii_ts = &dp83640->mii_ts;
1442
+	phydev->priv = dp83640;
1443
+
1444
+	spin_lock_init(&dp83640->rx_lock);
1445
+	skb_queue_head_init(&dp83640->rx_queue);
1446
+	skb_queue_head_init(&dp83640->tx_queue);
1447
+
1448
+	dp83640->clock = clock;
1449
+
1450
+	if (choose_this_phy(clock, phydev)) {
1451
+		clock->chosen = dp83640;
1452
+		clock->ptp_clock = ptp_clock_register(&clock->caps,
1453
+						      &phydev->mdio.dev);
1454
+		if (IS_ERR(clock->ptp_clock)) {
1455
+			err = PTR_ERR(clock->ptp_clock);
1456
+			goto no_register;
1457
+		}
1458
+	} else
1459
+		list_add_tail(&dp83640->list, &clock->phylist);
1460
+
1461
+	dp83640_clock_put(clock);
1462
+	return 0;
1463
+
1464
+no_register:
1465
+	clock->chosen = NULL;
1466
+	kfree(dp83640);
1467
+no_memory:
1468
+	dp83640_clock_put(clock);
1469
+no_clock:
1470
+	return err;
1471
+}
1472
+
1473
+static void dp83640_remove(struct phy_device *phydev)
1474
+{
1475
+	struct dp83640_clock *clock;
1476
+	struct list_head *this, *next;
1477
+	struct dp83640_private *tmp, *dp83640 = phydev->priv;
1478
+
1479
+	if (phydev->mdio.addr == BROADCAST_ADDR)
1480
+		return;
1481
+
1482
+	phydev->mii_ts = NULL;
1483
+
1484
+	enable_status_frames(phydev, false);
1485
+	cancel_delayed_work_sync(&dp83640->ts_work);
1486
+
1487
+	skb_queue_purge(&dp83640->rx_queue);
1488
+	skb_queue_purge(&dp83640->tx_queue);
1489
+
1490
+	clock = dp83640_clock_get(dp83640->clock);
1491
+
1492
+	if (dp83640 == clock->chosen) {
1493
+		ptp_clock_unregister(clock->ptp_clock);
1494
+		clock->chosen = NULL;
1495
+	} else {
1496
+		list_for_each_safe(this, next, &clock->phylist) {
1497
+			tmp = list_entry(this, struct dp83640_private, list);
1498
+			if (tmp == dp83640) {
1499
+				list_del_init(&tmp->list);
1500
+				break;
1501
+			}
1502
+		}
1503
+	}
1504
+
1505
+	dp83640_clock_put(clock);
1506
+	kfree(dp83640);
1507
+}
1508
+
15281509 static struct phy_driver dp83640_driver = {
15291510 	.phy_id		= DP83640_PHY_ID,
15301511 	.phy_id_mask	= 0xfffffff0,
15311512 	.name		= "NatSemi DP83640",
1532
-	.features	= PHY_BASIC_FEATURES,
1533
-	.flags		= PHY_HAS_INTERRUPT,
1513
+	/* PHY_BASIC_FEATURES */
15341514 	.probe		= dp83640_probe,
15351515 	.remove		= dp83640_remove,
15361516 	.soft_reset	= dp83640_soft_reset,
15371517 	.config_init	= dp83640_config_init,
15381518 	.ack_interrupt  = dp83640_ack_interrupt,
15391519 	.config_intr    = dp83640_config_intr,
1540
-	.ts_info	= dp83640_ts_info,
1541
-	.hwtstamp	= dp83640_hwtstamp,
1542
-	.rxtstamp	= dp83640_rxtstamp,
1543
-	.txtstamp	= dp83640_txtstamp,
15441520 };
15451521 
15461522 static int __init dp83640_init(void)