~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,16 +1,23 @@
	1	+// SPDX-License-Identifier: GPL-2.0+
1	2	/*
2	3	* Core PHY library, taken from phy.c
3		- *
4		- * This program is free software; you can redistribute it and/or modify it
5		- * under the terms of the GNU General Public License as published by the
6		- * Free Software Foundation; either version 2 of the License, or (at your
7		- * option) any later version.
8	4	*/
9	5	#include <linux/export.h>
10	6	#include <linux/phy.h>
	7	+#include <linux/of.h>
11	8
	9	+/**
	10	+ * phy_speed_to_str - Return a string representing the PHY link speed
	11	+ *
	12	+ * @speed: Speed of the link
	13	+ */
12	14	const char *phy_speed_to_str(int speed)
13	15	{
	16	+ BUILD_BUG_ON_MSG(__ETHTOOL_LINK_MODE_MASK_NBITS != 92,
	17	+ "Enum ethtool_link_mode_bit_indices and phylib are out of sync. "
	18	+ "If a speed or mode has been added please update phy_speed_to_str "
	19	+ "and the PHY settings array.\n");
	20	+
14	21	switch (speed) {
15	22	case SPEED_10:
16	23	return "10Mbps";
..	..	@@ -38,6 +45,10 @@
38	45	return "56Gbps";
39	46	case SPEED_100000:
40	47	return "100Gbps";
	48	+ case SPEED_200000:
	49	+ return "200Gbps";
	50	+ case SPEED_400000:
	51	+ return "400Gbps";
41	52	case SPEED_UNKNOWN:
42	53	return "Unknown";
43	54	default:
..	..	@@ -46,6 +57,11 @@
46	57	}
47	58	EXPORT_SYMBOL_GPL(phy_speed_to_str);
48	59
	60	+/**
	61	+ * phy_duplex_to_str - Return string describing the duplex
	62	+ *
	63	+ * @duplex: Duplex setting to describe
	64	+ */
49	65	const char *phy_duplex_to_str(unsigned int duplex)
50	66	{
51	67	if (duplex == DUPLEX_HALF)
..	..	@@ -61,75 +77,112 @@
61	77	/* A mapping of all SUPPORTED settings to speed/duplex. This table
62	78	* must be grouped by speed and sorted in descending match priority
63	79	* - iow, descending speed. */
	80	+
	81	+#define PHY_SETTING(s, d, b) { .speed = SPEED_ ## s, .duplex = DUPLEX_ ## d, \
	82	+ .bit = ETHTOOL_LINK_MODE_ ## b ## _BIT}
	83	+
64	84	static const struct phy_setting settings[] = {
65		- {
66		- .speed = SPEED_10000,
67		- .duplex = DUPLEX_FULL,
68		- .bit = ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
69		- },
70		- {
71		- .speed = SPEED_10000,
72		- .duplex = DUPLEX_FULL,
73		- .bit = ETHTOOL_LINK_MODE_10000baseKX4_Full_BIT,
74		- },
75		- {
76		- .speed = SPEED_10000,
77		- .duplex = DUPLEX_FULL,
78		- .bit = ETHTOOL_LINK_MODE_10000baseT_Full_BIT,
79		- },
80		- {
81		- .speed = SPEED_2500,
82		- .duplex = DUPLEX_FULL,
83		- .bit = ETHTOOL_LINK_MODE_2500baseX_Full_BIT,
84		- },
85		- {
86		- .speed = SPEED_1000,
87		- .duplex = DUPLEX_FULL,
88		- .bit = ETHTOOL_LINK_MODE_1000baseKX_Full_BIT,
89		- },
90		- {
91		- .speed = SPEED_1000,
92		- .duplex = DUPLEX_FULL,
93		- .bit = ETHTOOL_LINK_MODE_1000baseX_Full_BIT,
94		- },
95		- {
96		- .speed = SPEED_1000,
97		- .duplex = DUPLEX_FULL,
98		- .bit = ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
99		- },
100		- {
101		- .speed = SPEED_1000,
102		- .duplex = DUPLEX_HALF,
103		- .bit = ETHTOOL_LINK_MODE_1000baseT_Half_BIT,
104		- },
105		- {
106		- .speed = SPEED_100,
107		- .duplex = DUPLEX_FULL,
108		- .bit = ETHTOOL_LINK_MODE_100baseT_Full_BIT,
109		- },
110		- {
111		- .speed = SPEED_100,
112		- .duplex = DUPLEX_HALF,
113		- .bit = ETHTOOL_LINK_MODE_100baseT_Half_BIT,
114		- },
115		- {
116		- .speed = SPEED_10,
117		- .duplex = DUPLEX_FULL,
118		- .bit = ETHTOOL_LINK_MODE_10baseT_Full_BIT,
119		- },
120		- {
121		- .speed = SPEED_10,
122		- .duplex = DUPLEX_HALF,
123		- .bit = ETHTOOL_LINK_MODE_10baseT_Half_BIT,
124		- },
	85	+ /* 400G */
	86	+ PHY_SETTING( 400000, FULL, 400000baseCR8_Full ),
	87	+ PHY_SETTING( 400000, FULL, 400000baseKR8_Full ),
	88	+ PHY_SETTING( 400000, FULL, 400000baseLR8_ER8_FR8_Full ),
	89	+ PHY_SETTING( 400000, FULL, 400000baseDR8_Full ),
	90	+ PHY_SETTING( 400000, FULL, 400000baseSR8_Full ),
	91	+ PHY_SETTING( 400000, FULL, 400000baseCR4_Full ),
	92	+ PHY_SETTING( 400000, FULL, 400000baseKR4_Full ),
	93	+ PHY_SETTING( 400000, FULL, 400000baseLR4_ER4_FR4_Full ),
	94	+ PHY_SETTING( 400000, FULL, 400000baseDR4_Full ),
	95	+ PHY_SETTING( 400000, FULL, 400000baseSR4_Full ),
	96	+ /* 200G */
	97	+ PHY_SETTING( 200000, FULL, 200000baseCR4_Full ),
	98	+ PHY_SETTING( 200000, FULL, 200000baseKR4_Full ),
	99	+ PHY_SETTING( 200000, FULL, 200000baseLR4_ER4_FR4_Full ),
	100	+ PHY_SETTING( 200000, FULL, 200000baseDR4_Full ),
	101	+ PHY_SETTING( 200000, FULL, 200000baseSR4_Full ),
	102	+ PHY_SETTING( 200000, FULL, 200000baseCR2_Full ),
	103	+ PHY_SETTING( 200000, FULL, 200000baseKR2_Full ),
	104	+ PHY_SETTING( 200000, FULL, 200000baseLR2_ER2_FR2_Full ),
	105	+ PHY_SETTING( 200000, FULL, 200000baseDR2_Full ),
	106	+ PHY_SETTING( 200000, FULL, 200000baseSR2_Full ),
	107	+ /* 100G */
	108	+ PHY_SETTING( 100000, FULL, 100000baseCR4_Full ),
	109	+ PHY_SETTING( 100000, FULL, 100000baseKR4_Full ),
	110	+ PHY_SETTING( 100000, FULL, 100000baseLR4_ER4_Full ),
	111	+ PHY_SETTING( 100000, FULL, 100000baseSR4_Full ),
	112	+ PHY_SETTING( 100000, FULL, 100000baseCR2_Full ),
	113	+ PHY_SETTING( 100000, FULL, 100000baseKR2_Full ),
	114	+ PHY_SETTING( 100000, FULL, 100000baseLR2_ER2_FR2_Full ),
	115	+ PHY_SETTING( 100000, FULL, 100000baseDR2_Full ),
	116	+ PHY_SETTING( 100000, FULL, 100000baseSR2_Full ),
	117	+ PHY_SETTING( 100000, FULL, 100000baseCR_Full ),
	118	+ PHY_SETTING( 100000, FULL, 100000baseKR_Full ),
	119	+ PHY_SETTING( 100000, FULL, 100000baseLR_ER_FR_Full ),
	120	+ PHY_SETTING( 100000, FULL, 100000baseDR_Full ),
	121	+ PHY_SETTING( 100000, FULL, 100000baseSR_Full ),
	122	+ /* 56G */
	123	+ PHY_SETTING( 56000, FULL, 56000baseCR4_Full ),
	124	+ PHY_SETTING( 56000, FULL, 56000baseKR4_Full ),
	125	+ PHY_SETTING( 56000, FULL, 56000baseLR4_Full ),
	126	+ PHY_SETTING( 56000, FULL, 56000baseSR4_Full ),
	127	+ /* 50G */
	128	+ PHY_SETTING( 50000, FULL, 50000baseCR2_Full ),
	129	+ PHY_SETTING( 50000, FULL, 50000baseKR2_Full ),
	130	+ PHY_SETTING( 50000, FULL, 50000baseSR2_Full ),
	131	+ PHY_SETTING( 50000, FULL, 50000baseCR_Full ),
	132	+ PHY_SETTING( 50000, FULL, 50000baseKR_Full ),
	133	+ PHY_SETTING( 50000, FULL, 50000baseLR_ER_FR_Full ),
	134	+ PHY_SETTING( 50000, FULL, 50000baseDR_Full ),
	135	+ PHY_SETTING( 50000, FULL, 50000baseSR_Full ),
	136	+ /* 40G */
	137	+ PHY_SETTING( 40000, FULL, 40000baseCR4_Full ),
	138	+ PHY_SETTING( 40000, FULL, 40000baseKR4_Full ),
	139	+ PHY_SETTING( 40000, FULL, 40000baseLR4_Full ),
	140	+ PHY_SETTING( 40000, FULL, 40000baseSR4_Full ),
	141	+ /* 25G */
	142	+ PHY_SETTING( 25000, FULL, 25000baseCR_Full ),
	143	+ PHY_SETTING( 25000, FULL, 25000baseKR_Full ),
	144	+ PHY_SETTING( 25000, FULL, 25000baseSR_Full ),
	145	+ /* 20G */
	146	+ PHY_SETTING( 20000, FULL, 20000baseKR2_Full ),
	147	+ PHY_SETTING( 20000, FULL, 20000baseMLD2_Full ),
	148	+ /* 10G */
	149	+ PHY_SETTING( 10000, FULL, 10000baseCR_Full ),
	150	+ PHY_SETTING( 10000, FULL, 10000baseER_Full ),
	151	+ PHY_SETTING( 10000, FULL, 10000baseKR_Full ),
	152	+ PHY_SETTING( 10000, FULL, 10000baseKX4_Full ),
	153	+ PHY_SETTING( 10000, FULL, 10000baseLR_Full ),
	154	+ PHY_SETTING( 10000, FULL, 10000baseLRM_Full ),
	155	+ PHY_SETTING( 10000, FULL, 10000baseR_FEC ),
	156	+ PHY_SETTING( 10000, FULL, 10000baseSR_Full ),
	157	+ PHY_SETTING( 10000, FULL, 10000baseT_Full ),
	158	+ /* 5G */
	159	+ PHY_SETTING( 5000, FULL, 5000baseT_Full ),
	160	+ /* 2.5G */
	161	+ PHY_SETTING( 2500, FULL, 2500baseT_Full ),
	162	+ PHY_SETTING( 2500, FULL, 2500baseX_Full ),
	163	+ /* 1G */
	164	+ PHY_SETTING( 1000, FULL, 1000baseT_Full ),
	165	+ PHY_SETTING( 1000, HALF, 1000baseT_Half ),
	166	+ PHY_SETTING( 1000, FULL, 1000baseT1_Full ),
	167	+ PHY_SETTING( 1000, FULL, 1000baseX_Full ),
	168	+ PHY_SETTING( 1000, FULL, 1000baseKX_Full ),
	169	+ /* 100M */
	170	+ PHY_SETTING( 100, FULL, 100baseT_Full ),
	171	+ PHY_SETTING( 100, FULL, 100baseT1_Full ),
	172	+ PHY_SETTING( 100, HALF, 100baseT_Half ),
	173	+ PHY_SETTING( 100, HALF, 100baseFX_Half ),
	174	+ PHY_SETTING( 100, FULL, 100baseFX_Full ),
	175	+ /* 10M */
	176	+ PHY_SETTING( 10, FULL, 10baseT_Full ),
	177	+ PHY_SETTING( 10, HALF, 10baseT_Half ),
125	178	};
	179	+#undef PHY_SETTING
126	180
127	181	/**
128	182	* phy_lookup_setting - lookup a PHY setting
129	183	* @speed: speed to match
130	184	* @duplex: duplex to match
131	185	* @mask: allowed link modes
132		- * @maxbit: bit size of link modes
133	186	* @exact: an exact match is required
134	187	*
135	188	* Search the settings array for a setting that matches the speed and
..	..	@@ -143,14 +196,14 @@
143	196	* they all fail, %NULL will be returned.
144	197	*/
145	198	const struct phy_setting *
146		-phy_lookup_setting(int speed, int duplex, const unsigned long *mask,
147		- size_t maxbit, bool exact)
	199	+phy_lookup_setting(int speed, int duplex, const unsigned long *mask, bool exact)
148	200	{
149	201	const struct phy_setting p, match = NULL, *last = NULL;
150	202	int i;
151	203
152	204	for (i = 0, p = settings; i < ARRAY_SIZE(settings); i++, p++) {
153		- if (p->bit < maxbit && test_bit(p->bit, mask)) {
	205	+ if (p->bit < __ETHTOOL_LINK_MODE_MASK_NBITS &&
	206	+ test_bit(p->bit, mask)) {
154	207	last = p;
155	208	if (p->speed == speed && p->duplex == duplex) {
156	209	/* Exact match for speed and duplex */
..	..	@@ -175,13 +228,13 @@
175	228	EXPORT_SYMBOL_GPL(phy_lookup_setting);
176	229
177	230	size_t phy_speeds(unsigned int *speeds, size_t size,
178		- unsigned long *mask, size_t maxbit)
	231	+ unsigned long *mask)
179	232	{
180	233	size_t count;
181	234	int i;
182	235
183	236	for (i = 0, count = 0; i < ARRAY_SIZE(settings) && count < size; i++)
184		- if (settings[i].bit < maxbit &&
	237	+ if (settings[i].bit < __ETHTOOL_LINK_MODE_MASK_NBITS &&
185	238	test_bit(settings[i].bit, mask) &&
186	239	(count == 0 \|\| speeds[count - 1] != settings[i].speed))
187	240	speeds[count++] = settings[i].speed;
..	..	@@ -189,8 +242,116 @@
189	242	return count;
190	243	}
191	244
	245	+static int __set_linkmode_max_speed(u32 max_speed, unsigned long *addr)
	246	+{
	247	+ const struct phy_setting *p;
	248	+ int i;
	249	+
	250	+ for (i = 0, p = settings; i < ARRAY_SIZE(settings); i++, p++) {
	251	+ if (p->speed > max_speed)
	252	+ linkmode_clear_bit(p->bit, addr);
	253	+ else
	254	+ break;
	255	+ }
	256	+
	257	+ return 0;
	258	+}
	259	+
	260	+static int __set_phy_supported(struct phy_device *phydev, u32 max_speed)
	261	+{
	262	+ return __set_linkmode_max_speed(max_speed, phydev->supported);
	263	+}
	264	+
192	265	/**
193		- * phy_resolve_aneg_linkmode - resolve the advertisements into phy settings
	266	+ * phy_set_max_speed - Set the maximum speed the PHY should support
	267	+ *
	268	+ * @phydev: The phy_device struct
	269	+ * @max_speed: Maximum speed
	270	+ *
	271	+ * The PHY might be more capable than the MAC. For example a Fast Ethernet
	272	+ * is connected to a 1G PHY. This function allows the MAC to indicate its
	273	+ * maximum speed, and so limit what the PHY will advertise.
	274	+ */
	275	+int phy_set_max_speed(struct phy_device *phydev, u32 max_speed)
	276	+{
	277	+ int err;
	278	+
	279	+ err = __set_phy_supported(phydev, max_speed);
	280	+ if (err)
	281	+ return err;
	282	+
	283	+ phy_advertise_supported(phydev);
	284	+
	285	+ return 0;
	286	+}
	287	+EXPORT_SYMBOL(phy_set_max_speed);
	288	+
	289	+void of_set_phy_supported(struct phy_device *phydev)
	290	+{
	291	+ struct device_node *node = phydev->mdio.dev.of_node;
	292	+ u32 max_speed;
	293	+
	294	+ if (!IS_ENABLED(CONFIG_OF_MDIO))
	295	+ return;
	296	+
	297	+ if (!node)
	298	+ return;
	299	+
	300	+ if (!of_property_read_u32(node, "max-speed", &max_speed))
	301	+ __set_phy_supported(phydev, max_speed);
	302	+}
	303	+
	304	+void of_set_phy_eee_broken(struct phy_device *phydev)
	305	+{
	306	+ struct device_node *node = phydev->mdio.dev.of_node;
	307	+ u32 broken = 0;
	308	+
	309	+ if (!IS_ENABLED(CONFIG_OF_MDIO))
	310	+ return;
	311	+
	312	+ if (!node)
	313	+ return;
	314	+
	315	+ if (of_property_read_bool(node, "eee-broken-100tx"))
	316	+ broken \|= MDIO_EEE_100TX;
	317	+ if (of_property_read_bool(node, "eee-broken-1000t"))
	318	+ broken \|= MDIO_EEE_1000T;
	319	+ if (of_property_read_bool(node, "eee-broken-10gt"))
	320	+ broken \|= MDIO_EEE_10GT;
	321	+ if (of_property_read_bool(node, "eee-broken-1000kx"))
	322	+ broken \|= MDIO_EEE_1000KX;
	323	+ if (of_property_read_bool(node, "eee-broken-10gkx4"))
	324	+ broken \|= MDIO_EEE_10GKX4;
	325	+ if (of_property_read_bool(node, "eee-broken-10gkr"))
	326	+ broken \|= MDIO_EEE_10GKR;
	327	+
	328	+ phydev->eee_broken_modes = broken;
	329	+}
	330	+
	331	+/**
	332	+ * phy_resolve_aneg_pause - Determine pause autoneg results
	333	+ *
	334	+ * @phydev: The phy_device struct
	335	+ *
	336	+ * Once autoneg has completed the local pause settings can be
	337	+ * resolved. Determine if pause and asymmetric pause should be used
	338	+ * by the MAC.
	339	+ */
	340	+
	341	+void phy_resolve_aneg_pause(struct phy_device *phydev)
	342	+{
	343	+ if (phydev->duplex == DUPLEX_FULL) {
	344	+ phydev->pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
	345	+ phydev->lp_advertising);
	346	+ phydev->asym_pause = linkmode_test_bit(
	347	+ ETHTOOL_LINK_MODE_Asym_Pause_BIT,
	348	+ phydev->lp_advertising);
	349	+ }
	350	+}
	351	+EXPORT_SYMBOL_GPL(phy_resolve_aneg_pause);
	352	+
	353	+/**
	354	+ * phy_resolve_aneg_linkmode - resolve the advertisements into PHY settings
194	355	* @phydev: The phy_device struct
195	356	*
196	357	* Resolve our and the link partner advertisements into their corresponding
..	..	@@ -199,38 +360,87 @@
199	360	*/
200	361	void phy_resolve_aneg_linkmode(struct phy_device *phydev)
201	362	{
202		- u32 common = phydev->lp_advertising & phydev->advertising;
	363	+ __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
	364	+ int i;
203	365
204		- if (common & ADVERTISED_10000baseT_Full) {
205		- phydev->speed = SPEED_10000;
206		- phydev->duplex = DUPLEX_FULL;
207		- } else if (common & ADVERTISED_1000baseT_Full) {
208		- phydev->speed = SPEED_1000;
209		- phydev->duplex = DUPLEX_FULL;
210		- } else if (common & ADVERTISED_1000baseT_Half) {
211		- phydev->speed = SPEED_1000;
212		- phydev->duplex = DUPLEX_HALF;
213		- } else if (common & ADVERTISED_100baseT_Full) {
214		- phydev->speed = SPEED_100;
215		- phydev->duplex = DUPLEX_FULL;
216		- } else if (common & ADVERTISED_100baseT_Half) {
217		- phydev->speed = SPEED_100;
218		- phydev->duplex = DUPLEX_HALF;
219		- } else if (common & ADVERTISED_10baseT_Full) {
220		- phydev->speed = SPEED_10;
221		- phydev->duplex = DUPLEX_FULL;
222		- } else if (common & ADVERTISED_10baseT_Half) {
223		- phydev->speed = SPEED_10;
224		- phydev->duplex = DUPLEX_HALF;
225		- }
	366	+ linkmode_and(common, phydev->lp_advertising, phydev->advertising);
226	367
227		- if (phydev->duplex == DUPLEX_FULL) {
228		- phydev->pause = !!(phydev->lp_advertising & ADVERTISED_Pause);
229		- phydev->asym_pause = !!(phydev->lp_advertising &
230		- ADVERTISED_Asym_Pause);
231		- }
	368	+ for (i = 0; i < ARRAY_SIZE(settings); i++)
	369	+ if (test_bit(settings[i].bit, common)) {
	370	+ phydev->speed = settings[i].speed;
	371	+ phydev->duplex = settings[i].duplex;
	372	+ break;
	373	+ }
	374	+
	375	+ phy_resolve_aneg_pause(phydev);
232	376	}
233	377	EXPORT_SYMBOL_GPL(phy_resolve_aneg_linkmode);
	378	+
	379	+/**
	380	+ * phy_check_downshift - check whether downshift occurred
	381	+ * @phydev: The phy_device struct
	382	+ *
	383	+ * Check whether a downshift to a lower speed occurred. If this should be the
	384	+ * case warn the user.
	385	+ * Prerequisite for detecting downshift is that PHY driver implements the
	386	+ * read_status callback and sets phydev->speed to the actual link speed.
	387	+ */
	388	+void phy_check_downshift(struct phy_device *phydev)
	389	+{
	390	+ __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
	391	+ int i, speed = SPEED_UNKNOWN;
	392	+
	393	+ phydev->downshifted_rate = 0;
	394	+
	395	+ if (phydev->autoneg == AUTONEG_DISABLE \|\|
	396	+ phydev->speed == SPEED_UNKNOWN)
	397	+ return;
	398	+
	399	+ linkmode_and(common, phydev->lp_advertising, phydev->advertising);
	400	+
	401	+ for (i = 0; i < ARRAY_SIZE(settings); i++)
	402	+ if (test_bit(settings[i].bit, common)) {
	403	+ speed = settings[i].speed;
	404	+ break;
	405	+ }
	406	+
	407	+ if (speed == SPEED_UNKNOWN \|\| phydev->speed >= speed)
	408	+ return;
	409	+
	410	+ phydev_warn(phydev, "Downshift occurred from negotiated speed %s to actual speed %s, check cabling!\n",
	411	+ phy_speed_to_str(speed), phy_speed_to_str(phydev->speed));
	412	+
	413	+ phydev->downshifted_rate = 1;
	414	+}
	415	+EXPORT_SYMBOL_GPL(phy_check_downshift);
	416	+
	417	+static int phy_resolve_min_speed(struct phy_device *phydev, bool fdx_only)
	418	+{
	419	+ __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
	420	+ int i = ARRAY_SIZE(settings);
	421	+
	422	+ linkmode_and(common, phydev->lp_advertising, phydev->advertising);
	423	+
	424	+ while (--i >= 0) {
	425	+ if (test_bit(settings[i].bit, common)) {
	426	+ if (fdx_only && settings[i].duplex != DUPLEX_FULL)
	427	+ continue;
	428	+ return settings[i].speed;
	429	+ }
	430	+ }
	431	+
	432	+ return SPEED_UNKNOWN;
	433	+}
	434	+
	435	+int phy_speed_down_core(struct phy_device *phydev)
	436	+{
	437	+ int min_common_speed = phy_resolve_min_speed(phydev, true);
	438	+
	439	+ if (min_common_speed == SPEED_UNKNOWN)
	440	+ return -EINVAL;
	441	+
	442	+ return __set_linkmode_max_speed(min_common_speed, phydev->advertising);
	443	+}
234	444
235	445	static void mmd_phy_indirect(struct mii_bus *bus, int phy_addr, int devad,
236	446	u16 regnum)
..	..	@@ -247,41 +457,96 @@
247	457	}
248	458
249	459	/**
250		- * phy_read_mmd - Convenience function for reading a register
	460	+ * __phy_read_mmd - Convenience function for reading a register
251	461	* from an MMD on a given PHY.
252	462	* @phydev: The phy_device struct
253	463	* @devad: The MMD to read from (0..31)
254	464	* @regnum: The register on the MMD to read (0..65535)
255	465	*
256		- * Same rules as for phy_read();
	466	+ * Same rules as for __phy_read();
257	467	*/
258		-int phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
	468	+int __phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
259	469	{
260	470	int val;
261	471
262	472	if (regnum > (u16)~0 \|\| devad > 32)
263	473	return -EINVAL;
264	474
265		- if (phydev->drv->read_mmd) {
	475	+ if (phydev->drv && phydev->drv->read_mmd) {
266	476	val = phydev->drv->read_mmd(phydev, devad, regnum);
267	477	} else if (phydev->is_c45) {
268		- u32 addr = MII_ADDR_C45 \| (devad << 16) \| (regnum & 0xffff);
269		-
270		- val = mdiobus_read(phydev->mdio.bus, phydev->mdio.addr, addr);
	478	+ val = __mdiobus_c45_read(phydev->mdio.bus, phydev->mdio.addr,
	479	+ devad, regnum);
271	480	} else {
272	481	struct mii_bus *bus = phydev->mdio.bus;
273	482	int phy_addr = phydev->mdio.addr;
274	483
275		- mutex_lock(&bus->mdio_lock);
276	484	mmd_phy_indirect(bus, phy_addr, devad, regnum);
277	485
278	486	/* Read the content of the MMD's selected register */
279	487	val = __mdiobus_read(bus, phy_addr, MII_MMD_DATA);
280		- mutex_unlock(&bus->mdio_lock);
281	488	}
282	489	return val;
283	490	}
	491	+EXPORT_SYMBOL(__phy_read_mmd);
	492	+
	493	+/**
	494	+ * phy_read_mmd - Convenience function for reading a register
	495	+ * from an MMD on a given PHY.
	496	+ * @phydev: The phy_device struct
	497	+ * @devad: The MMD to read from
	498	+ * @regnum: The register on the MMD to read
	499	+ *
	500	+ * Same rules as for phy_read();
	501	+ */
	502	+int phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
	503	+{
	504	+ int ret;
	505	+
	506	+ phy_lock_mdio_bus(phydev);
	507	+ ret = __phy_read_mmd(phydev, devad, regnum);
	508	+ phy_unlock_mdio_bus(phydev);
	509	+
	510	+ return ret;
	511	+}
284	512	EXPORT_SYMBOL(phy_read_mmd);
	513	+
	514	+/**
	515	+ * __phy_write_mmd - Convenience function for writing a register
	516	+ * on an MMD on a given PHY.
	517	+ * @phydev: The phy_device struct
	518	+ * @devad: The MMD to read from
	519	+ * @regnum: The register on the MMD to read
	520	+ * @val: value to write to @regnum
	521	+ *
	522	+ * Same rules as for __phy_write();
	523	+ */
	524	+int __phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
	525	+{
	526	+ int ret;
	527	+
	528	+ if (regnum > (u16)~0 \|\| devad > 32)
	529	+ return -EINVAL;
	530	+
	531	+ if (phydev->drv && phydev->drv->write_mmd) {
	532	+ ret = phydev->drv->write_mmd(phydev, devad, regnum, val);
	533	+ } else if (phydev->is_c45) {
	534	+ ret = __mdiobus_c45_write(phydev->mdio.bus, phydev->mdio.addr,
	535	+ devad, regnum, val);
	536	+ } else {
	537	+ struct mii_bus *bus = phydev->mdio.bus;
	538	+ int phy_addr = phydev->mdio.addr;
	539	+
	540	+ mmd_phy_indirect(bus, phy_addr, devad, regnum);
	541	+
	542	+ /* Write the data into MMD's selected register */
	543	+ __mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
	544	+
	545	+ ret = 0;
	546	+ }
	547	+ return ret;
	548	+}
	549	+EXPORT_SYMBOL(__phy_write_mmd);
285	550
286	551	/**
287	552	* phy_write_mmd - Convenience function for writing a register
..	..	@@ -297,52 +562,55 @@
297	562	{
298	563	int ret;
299	564
300		- if (regnum > (u16)~0 \|\| devad > 32)
301		- return -EINVAL;
	565	+ phy_lock_mdio_bus(phydev);
	566	+ ret = __phy_write_mmd(phydev, devad, regnum, val);
	567	+ phy_unlock_mdio_bus(phydev);
302	568
303		- if (phydev->drv->write_mmd) {
304		- ret = phydev->drv->write_mmd(phydev, devad, regnum, val);
305		- } else if (phydev->is_c45) {
306		- u32 addr = MII_ADDR_C45 \| (devad << 16) \| (regnum & 0xffff);
307		-
308		- ret = mdiobus_write(phydev->mdio.bus, phydev->mdio.addr,
309		- addr, val);
310		- } else {
311		- struct mii_bus *bus = phydev->mdio.bus;
312		- int phy_addr = phydev->mdio.addr;
313		-
314		- mutex_lock(&bus->mdio_lock);
315		- mmd_phy_indirect(bus, phy_addr, devad, regnum);
316		-
317		- /* Write the data into MMD's selected register */
318		- __mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
319		- mutex_unlock(&bus->mdio_lock);
320		-
321		- ret = 0;
322		- }
323	569	return ret;
324	570	}
325	571	EXPORT_SYMBOL(phy_write_mmd);
326	572
327	573	/**
328		- * __phy_modify() - Convenience function for modifying a PHY register
329		- * @phydev: a pointer to a &struct phy_device
330		- * @regnum: register number
	574	+ * phy_modify_changed - Function for modifying a PHY register
	575	+ * @phydev: the phy_device struct
	576	+ * @regnum: register number to modify
331	577	* @mask: bit mask of bits to clear
332		- * @set: bit mask of bits to set
	578	+ * @set: new value of bits set in mask to write to @regnum
333	579	*
334		- * Unlocked helper function which allows a PHY register to be modified as
335		- * new register value = (old register value & ~mask) \| set
	580	+ * NOTE: MUST NOT be called from interrupt context,
	581	+ * because the bus read/write functions may wait for an interrupt
	582	+ * to conclude the operation.
	583	+ *
	584	+ * Returns negative errno, 0 if there was no change, and 1 in case of change
	585	+ */
	586	+int phy_modify_changed(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
	587	+{
	588	+ int ret;
	589	+
	590	+ phy_lock_mdio_bus(phydev);
	591	+ ret = __phy_modify_changed(phydev, regnum, mask, set);
	592	+ phy_unlock_mdio_bus(phydev);
	593	+
	594	+ return ret;
	595	+}
	596	+EXPORT_SYMBOL_GPL(phy_modify_changed);
	597	+
	598	+/**
	599	+ * __phy_modify - Convenience function for modifying a PHY register
	600	+ * @phydev: the phy_device struct
	601	+ * @regnum: register number to modify
	602	+ * @mask: bit mask of bits to clear
	603	+ * @set: new value of bits set in mask to write to @regnum
	604	+ *
	605	+ * NOTE: MUST NOT be called from interrupt context,
	606	+ * because the bus read/write functions may wait for an interrupt
	607	+ * to conclude the operation.
336	608	*/
337	609	int __phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
338	610	{
339	611	int ret;
340	612
341		- ret = __phy_read(phydev, regnum);
342		- if (ret < 0)
343		- return ret;
344		-
345		- ret = __phy_write(phydev, regnum, (ret & ~mask) \| set);
	613	+ ret = __phy_modify_changed(phydev, regnum, mask, set);
346	614
347	615	return ret < 0 ? ret : 0;
348	616	}
..	..	@@ -363,21 +631,134 @@
363	631	{
364	632	int ret;
365	633
366		- mutex_lock(&phydev->mdio.bus->mdio_lock);
	634	+ phy_lock_mdio_bus(phydev);
367	635	ret = __phy_modify(phydev, regnum, mask, set);
368		- mutex_unlock(&phydev->mdio.bus->mdio_lock);
	636	+ phy_unlock_mdio_bus(phydev);
369	637
370	638	return ret;
371	639	}
372	640	EXPORT_SYMBOL_GPL(phy_modify);
373	641
	642	+/**
	643	+ * __phy_modify_mmd_changed - Function for modifying a register on MMD
	644	+ * @phydev: the phy_device struct
	645	+ * @devad: the MMD containing register to modify
	646	+ * @regnum: register number to modify
	647	+ * @mask: bit mask of bits to clear
	648	+ * @set: new value of bits set in mask to write to @regnum
	649	+ *
	650	+ * Unlocked helper function which allows a MMD register to be modified as
	651	+ * new register value = (old register value & ~mask) \| set
	652	+ *
	653	+ * Returns negative errno, 0 if there was no change, and 1 in case of change
	654	+ */
	655	+int __phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
	656	+ u16 mask, u16 set)
	657	+{
	658	+ int new, ret;
	659	+
	660	+ ret = __phy_read_mmd(phydev, devad, regnum);
	661	+ if (ret < 0)
	662	+ return ret;
	663	+
	664	+ new = (ret & ~mask) \| set;
	665	+ if (new == ret)
	666	+ return 0;
	667	+
	668	+ ret = __phy_write_mmd(phydev, devad, regnum, new);
	669	+
	670	+ return ret < 0 ? ret : 1;
	671	+}
	672	+EXPORT_SYMBOL_GPL(__phy_modify_mmd_changed);
	673	+
	674	+/**
	675	+ * phy_modify_mmd_changed - Function for modifying a register on MMD
	676	+ * @phydev: the phy_device struct
	677	+ * @devad: the MMD containing register to modify
	678	+ * @regnum: register number to modify
	679	+ * @mask: bit mask of bits to clear
	680	+ * @set: new value of bits set in mask to write to @regnum
	681	+ *
	682	+ * NOTE: MUST NOT be called from interrupt context,
	683	+ * because the bus read/write functions may wait for an interrupt
	684	+ * to conclude the operation.
	685	+ *
	686	+ * Returns negative errno, 0 if there was no change, and 1 in case of change
	687	+ */
	688	+int phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
	689	+ u16 mask, u16 set)
	690	+{
	691	+ int ret;
	692	+
	693	+ phy_lock_mdio_bus(phydev);
	694	+ ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
	695	+ phy_unlock_mdio_bus(phydev);
	696	+
	697	+ return ret;
	698	+}
	699	+EXPORT_SYMBOL_GPL(phy_modify_mmd_changed);
	700	+
	701	+/**
	702	+ * __phy_modify_mmd - Convenience function for modifying a register on MMD
	703	+ * @phydev: the phy_device struct
	704	+ * @devad: the MMD containing register to modify
	705	+ * @regnum: register number to modify
	706	+ * @mask: bit mask of bits to clear
	707	+ * @set: new value of bits set in mask to write to @regnum
	708	+ *
	709	+ * NOTE: MUST NOT be called from interrupt context,
	710	+ * because the bus read/write functions may wait for an interrupt
	711	+ * to conclude the operation.
	712	+ */
	713	+int __phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
	714	+ u16 mask, u16 set)
	715	+{
	716	+ int ret;
	717	+
	718	+ ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
	719	+
	720	+ return ret < 0 ? ret : 0;
	721	+}
	722	+EXPORT_SYMBOL_GPL(__phy_modify_mmd);
	723	+
	724	+/**
	725	+ * phy_modify_mmd - Convenience function for modifying a register on MMD
	726	+ * @phydev: the phy_device struct
	727	+ * @devad: the MMD containing register to modify
	728	+ * @regnum: register number to modify
	729	+ * @mask: bit mask of bits to clear
	730	+ * @set: new value of bits set in mask to write to @regnum
	731	+ *
	732	+ * NOTE: MUST NOT be called from interrupt context,
	733	+ * because the bus read/write functions may wait for an interrupt
	734	+ * to conclude the operation.
	735	+ */
	736	+int phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
	737	+ u16 mask, u16 set)
	738	+{
	739	+ int ret;
	740	+
	741	+ phy_lock_mdio_bus(phydev);
	742	+ ret = __phy_modify_mmd(phydev, devad, regnum, mask, set);
	743	+ phy_unlock_mdio_bus(phydev);
	744	+
	745	+ return ret;
	746	+}
	747	+EXPORT_SYMBOL_GPL(phy_modify_mmd);
	748	+
374	749	static int __phy_read_page(struct phy_device *phydev)
375	750	{
	751	+ if (WARN_ONCE(!phydev->drv->read_page, "read_page callback not available, PHY driver not loaded?\n"))
	752	+ return -EOPNOTSUPP;
	753	+
376	754	return phydev->drv->read_page(phydev);
377	755	}
378	756
379	757	static int __phy_write_page(struct phy_device *phydev, int page)
380	758	{
	759	+ if (WARN_ONCE(!phydev->drv->write_page, "write_page callback not available, PHY driver not loaded?\n"))
	760	+ return -EOPNOTSUPP;
	761	+
381	762	return phydev->drv->write_page(phydev, page);
382	763	}
383	764
..	..	@@ -391,7 +772,7 @@
391	772	*/
392	773	int phy_save_page(struct phy_device *phydev)
393	774	{
394		- mutex_lock(&phydev->mdio.bus->mdio_lock);
	775	+ phy_lock_mdio_bus(phydev);
395	776	return __phy_read_page(phydev);
396	777	}
397	778	EXPORT_SYMBOL_GPL(phy_save_page);
..	..	@@ -458,7 +839,7 @@
458	839	ret = oldpage;
459	840	}
460	841
461		- mutex_unlock(&phydev->mdio.bus->mdio_lock);
	842	+ phy_unlock_mdio_bus(phydev);
462	843
463	844	return ret;
464	845	}
..	..	@@ -506,6 +887,29 @@
506	887	EXPORT_SYMBOL(phy_write_paged);
507	888
508	889	/**
	890	+ * phy_modify_paged_changed() - Function for modifying a paged register
	891	+ * @phydev: a pointer to a &struct phy_device
	892	+ * @page: the page for the phy
	893	+ * @regnum: register number
	894	+ * @mask: bit mask of bits to clear
	895	+ * @set: bit mask of bits to set
	896	+ *
	897	+ * Returns negative errno, 0 if there was no change, and 1 in case of change
	898	+ */
	899	+int phy_modify_paged_changed(struct phy_device *phydev, int page, u32 regnum,
	900	+ u16 mask, u16 set)
	901	+{
	902	+ int ret = 0, oldpage;
	903	+
	904	+ oldpage = phy_select_page(phydev, page);
	905	+ if (oldpage >= 0)
	906	+ ret = __phy_modify_changed(phydev, regnum, mask, set);
	907	+
	908	+ return phy_restore_page(phydev, oldpage, ret);
	909	+}
	910	+EXPORT_SYMBOL(phy_modify_paged_changed);
	911	+
	912	+/**
509	913	* phy_modify_paged() - Convenience function for modifying a paged register
510	914	* @phydev: a pointer to a &struct phy_device
511	915	* @page: the page for the phy
..	..	@@ -518,12 +922,8 @@
518	922	int phy_modify_paged(struct phy_device *phydev, int page, u32 regnum,
519	923	u16 mask, u16 set)
520	924	{
521		- int ret = 0, oldpage;
	925	+ int ret = phy_modify_paged_changed(phydev, page, regnum, mask, set);
522	926
523		- oldpage = phy_select_page(phydev, page);
524		- if (oldpage >= 0)
525		- ret = __phy_modify(phydev, regnum, mask, set);
526		-
527		- return phy_restore_page(phydev, oldpage, ret);
	927	+ return ret < 0 ? ret : 0;
528	928	}
529	929	EXPORT_SYMBOL(phy_modify_paged);