1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * Core PHY library, taken from phy.c
4  */
5 #include <linux/export.h>
6 #include <linux/phy.h>
7 #include <linux/of.h>
8 
9 /**
10  * phy_speed_to_str - Return a string representing the PHY link speed
11  *
12  * @speed: Speed of the link
13  */
phy_speed_to_str(int speed)14 const char *phy_speed_to_str(int speed)
15 {
16 	BUILD_BUG_ON_MSG(__ETHTOOL_LINK_MODE_MASK_NBITS != 102,
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 
21 	switch (speed) {
22 	case SPEED_10:
23 		return "10Mbps";
24 	case SPEED_100:
25 		return "100Mbps";
26 	case SPEED_1000:
27 		return "1Gbps";
28 	case SPEED_2500:
29 		return "2.5Gbps";
30 	case SPEED_5000:
31 		return "5Gbps";
32 	case SPEED_10000:
33 		return "10Gbps";
34 	case SPEED_14000:
35 		return "14Gbps";
36 	case SPEED_20000:
37 		return "20Gbps";
38 	case SPEED_25000:
39 		return "25Gbps";
40 	case SPEED_40000:
41 		return "40Gbps";
42 	case SPEED_50000:
43 		return "50Gbps";
44 	case SPEED_56000:
45 		return "56Gbps";
46 	case SPEED_100000:
47 		return "100Gbps";
48 	case SPEED_200000:
49 		return "200Gbps";
50 	case SPEED_400000:
51 		return "400Gbps";
52 	case SPEED_800000:
53 		return "800Gbps";
54 	case SPEED_UNKNOWN:
55 		return "Unknown";
56 	default:
57 		return "Unsupported (update phy-core.c)";
58 	}
59 }
60 EXPORT_SYMBOL_GPL(phy_speed_to_str);
61 
62 /**
63  * phy_duplex_to_str - Return string describing the duplex
64  *
65  * @duplex: Duplex setting to describe
66  */
phy_duplex_to_str(unsigned int duplex)67 const char *phy_duplex_to_str(unsigned int duplex)
68 {
69 	if (duplex == DUPLEX_HALF)
70 		return "Half";
71 	if (duplex == DUPLEX_FULL)
72 		return "Full";
73 	if (duplex == DUPLEX_UNKNOWN)
74 		return "Unknown";
75 	return "Unsupported (update phy-core.c)";
76 }
77 EXPORT_SYMBOL_GPL(phy_duplex_to_str);
78 
79 /**
80  * phy_rate_matching_to_str - Return a string describing the rate matching
81  *
82  * @rate_matching: Type of rate matching to describe
83  */
phy_rate_matching_to_str(int rate_matching)84 const char *phy_rate_matching_to_str(int rate_matching)
85 {
86 	switch (rate_matching) {
87 	case RATE_MATCH_NONE:
88 		return "none";
89 	case RATE_MATCH_PAUSE:
90 		return "pause";
91 	case RATE_MATCH_CRS:
92 		return "crs";
93 	case RATE_MATCH_OPEN_LOOP:
94 		return "open-loop";
95 	}
96 	return "Unsupported (update phy-core.c)";
97 }
98 EXPORT_SYMBOL_GPL(phy_rate_matching_to_str);
99 
100 /**
101  * phy_interface_num_ports - Return the number of links that can be carried by
102  *			     a given MAC-PHY physical link. Returns 0 if this is
103  *			     unknown, the number of links else.
104  *
105  * @interface: The interface mode we want to get the number of ports
106  */
phy_interface_num_ports(phy_interface_t interface)107 int phy_interface_num_ports(phy_interface_t interface)
108 {
109 	switch (interface) {
110 	case PHY_INTERFACE_MODE_NA:
111 		return 0;
112 	case PHY_INTERFACE_MODE_INTERNAL:
113 	case PHY_INTERFACE_MODE_MII:
114 	case PHY_INTERFACE_MODE_GMII:
115 	case PHY_INTERFACE_MODE_TBI:
116 	case PHY_INTERFACE_MODE_REVMII:
117 	case PHY_INTERFACE_MODE_RMII:
118 	case PHY_INTERFACE_MODE_REVRMII:
119 	case PHY_INTERFACE_MODE_RGMII:
120 	case PHY_INTERFACE_MODE_RGMII_ID:
121 	case PHY_INTERFACE_MODE_RGMII_RXID:
122 	case PHY_INTERFACE_MODE_RGMII_TXID:
123 	case PHY_INTERFACE_MODE_RTBI:
124 	case PHY_INTERFACE_MODE_XGMII:
125 	case PHY_INTERFACE_MODE_XLGMII:
126 	case PHY_INTERFACE_MODE_MOCA:
127 	case PHY_INTERFACE_MODE_TRGMII:
128 	case PHY_INTERFACE_MODE_USXGMII:
129 	case PHY_INTERFACE_MODE_SGMII:
130 	case PHY_INTERFACE_MODE_SMII:
131 	case PHY_INTERFACE_MODE_1000BASEX:
132 	case PHY_INTERFACE_MODE_2500BASEX:
133 	case PHY_INTERFACE_MODE_5GBASER:
134 	case PHY_INTERFACE_MODE_10GBASER:
135 	case PHY_INTERFACE_MODE_25GBASER:
136 	case PHY_INTERFACE_MODE_10GKR:
137 	case PHY_INTERFACE_MODE_100BASEX:
138 	case PHY_INTERFACE_MODE_RXAUI:
139 	case PHY_INTERFACE_MODE_XAUI:
140 	case PHY_INTERFACE_MODE_1000BASEKX:
141 		return 1;
142 	case PHY_INTERFACE_MODE_QSGMII:
143 	case PHY_INTERFACE_MODE_QUSGMII:
144 		return 4;
145 	case PHY_INTERFACE_MODE_PSGMII:
146 		return 5;
147 	case PHY_INTERFACE_MODE_MAX:
148 		WARN_ONCE(1, "PHY_INTERFACE_MODE_MAX isn't a valid interface mode");
149 		return 0;
150 	}
151 	return 0;
152 }
153 EXPORT_SYMBOL_GPL(phy_interface_num_ports);
154 
155 /* A mapping of all SUPPORTED settings to speed/duplex.  This table
156  * must be grouped by speed and sorted in descending match priority
157  * - iow, descending speed.
158  */
159 
160 #define PHY_SETTING(s, d, b) { .speed = SPEED_ ## s, .duplex = DUPLEX_ ## d, \
161 			       .bit = ETHTOOL_LINK_MODE_ ## b ## _BIT}
162 
163 static const struct phy_setting settings[] = {
164 	/* 800G */
165 	PHY_SETTING( 800000, FULL, 800000baseCR8_Full		),
166 	PHY_SETTING( 800000, FULL, 800000baseKR8_Full		),
167 	PHY_SETTING( 800000, FULL, 800000baseDR8_Full		),
168 	PHY_SETTING( 800000, FULL, 800000baseDR8_2_Full		),
169 	PHY_SETTING( 800000, FULL, 800000baseSR8_Full		),
170 	PHY_SETTING( 800000, FULL, 800000baseVR8_Full		),
171 	/* 400G */
172 	PHY_SETTING( 400000, FULL, 400000baseCR8_Full		),
173 	PHY_SETTING( 400000, FULL, 400000baseKR8_Full		),
174 	PHY_SETTING( 400000, FULL, 400000baseLR8_ER8_FR8_Full	),
175 	PHY_SETTING( 400000, FULL, 400000baseDR8_Full		),
176 	PHY_SETTING( 400000, FULL, 400000baseSR8_Full		),
177 	PHY_SETTING( 400000, FULL, 400000baseCR4_Full		),
178 	PHY_SETTING( 400000, FULL, 400000baseKR4_Full		),
179 	PHY_SETTING( 400000, FULL, 400000baseLR4_ER4_FR4_Full	),
180 	PHY_SETTING( 400000, FULL, 400000baseDR4_Full		),
181 	PHY_SETTING( 400000, FULL, 400000baseSR4_Full		),
182 	/* 200G */
183 	PHY_SETTING( 200000, FULL, 200000baseCR4_Full		),
184 	PHY_SETTING( 200000, FULL, 200000baseKR4_Full		),
185 	PHY_SETTING( 200000, FULL, 200000baseLR4_ER4_FR4_Full	),
186 	PHY_SETTING( 200000, FULL, 200000baseDR4_Full		),
187 	PHY_SETTING( 200000, FULL, 200000baseSR4_Full		),
188 	PHY_SETTING( 200000, FULL, 200000baseCR2_Full		),
189 	PHY_SETTING( 200000, FULL, 200000baseKR2_Full		),
190 	PHY_SETTING( 200000, FULL, 200000baseLR2_ER2_FR2_Full	),
191 	PHY_SETTING( 200000, FULL, 200000baseDR2_Full		),
192 	PHY_SETTING( 200000, FULL, 200000baseSR2_Full		),
193 	/* 100G */
194 	PHY_SETTING( 100000, FULL, 100000baseCR4_Full		),
195 	PHY_SETTING( 100000, FULL, 100000baseKR4_Full		),
196 	PHY_SETTING( 100000, FULL, 100000baseLR4_ER4_Full	),
197 	PHY_SETTING( 100000, FULL, 100000baseSR4_Full		),
198 	PHY_SETTING( 100000, FULL, 100000baseCR2_Full		),
199 	PHY_SETTING( 100000, FULL, 100000baseKR2_Full		),
200 	PHY_SETTING( 100000, FULL, 100000baseLR2_ER2_FR2_Full	),
201 	PHY_SETTING( 100000, FULL, 100000baseDR2_Full		),
202 	PHY_SETTING( 100000, FULL, 100000baseSR2_Full		),
203 	PHY_SETTING( 100000, FULL, 100000baseCR_Full		),
204 	PHY_SETTING( 100000, FULL, 100000baseKR_Full		),
205 	PHY_SETTING( 100000, FULL, 100000baseLR_ER_FR_Full	),
206 	PHY_SETTING( 100000, FULL, 100000baseDR_Full		),
207 	PHY_SETTING( 100000, FULL, 100000baseSR_Full		),
208 	/* 56G */
209 	PHY_SETTING(  56000, FULL,  56000baseCR4_Full	  	),
210 	PHY_SETTING(  56000, FULL,  56000baseKR4_Full	  	),
211 	PHY_SETTING(  56000, FULL,  56000baseLR4_Full	  	),
212 	PHY_SETTING(  56000, FULL,  56000baseSR4_Full	  	),
213 	/* 50G */
214 	PHY_SETTING(  50000, FULL,  50000baseCR2_Full		),
215 	PHY_SETTING(  50000, FULL,  50000baseKR2_Full		),
216 	PHY_SETTING(  50000, FULL,  50000baseSR2_Full		),
217 	PHY_SETTING(  50000, FULL,  50000baseCR_Full		),
218 	PHY_SETTING(  50000, FULL,  50000baseKR_Full		),
219 	PHY_SETTING(  50000, FULL,  50000baseLR_ER_FR_Full	),
220 	PHY_SETTING(  50000, FULL,  50000baseDR_Full		),
221 	PHY_SETTING(  50000, FULL,  50000baseSR_Full		),
222 	/* 40G */
223 	PHY_SETTING(  40000, FULL,  40000baseCR4_Full		),
224 	PHY_SETTING(  40000, FULL,  40000baseKR4_Full		),
225 	PHY_SETTING(  40000, FULL,  40000baseLR4_Full		),
226 	PHY_SETTING(  40000, FULL,  40000baseSR4_Full		),
227 	/* 25G */
228 	PHY_SETTING(  25000, FULL,  25000baseCR_Full		),
229 	PHY_SETTING(  25000, FULL,  25000baseKR_Full		),
230 	PHY_SETTING(  25000, FULL,  25000baseSR_Full		),
231 	/* 20G */
232 	PHY_SETTING(  20000, FULL,  20000baseKR2_Full		),
233 	PHY_SETTING(  20000, FULL,  20000baseMLD2_Full		),
234 	/* 10G */
235 	PHY_SETTING(  10000, FULL,  10000baseCR_Full		),
236 	PHY_SETTING(  10000, FULL,  10000baseER_Full		),
237 	PHY_SETTING(  10000, FULL,  10000baseKR_Full		),
238 	PHY_SETTING(  10000, FULL,  10000baseKX4_Full		),
239 	PHY_SETTING(  10000, FULL,  10000baseLR_Full		),
240 	PHY_SETTING(  10000, FULL,  10000baseLRM_Full		),
241 	PHY_SETTING(  10000, FULL,  10000baseR_FEC		),
242 	PHY_SETTING(  10000, FULL,  10000baseSR_Full		),
243 	PHY_SETTING(  10000, FULL,  10000baseT_Full		),
244 	/* 5G */
245 	PHY_SETTING(   5000, FULL,   5000baseT_Full		),
246 	/* 2.5G */
247 	PHY_SETTING(   2500, FULL,   2500baseT_Full		),
248 	PHY_SETTING(   2500, FULL,   2500baseX_Full		),
249 	/* 1G */
250 	PHY_SETTING(   1000, FULL,   1000baseT_Full		),
251 	PHY_SETTING(   1000, HALF,   1000baseT_Half		),
252 	PHY_SETTING(   1000, FULL,   1000baseT1_Full		),
253 	PHY_SETTING(   1000, FULL,   1000baseX_Full		),
254 	PHY_SETTING(   1000, FULL,   1000baseKX_Full		),
255 	/* 100M */
256 	PHY_SETTING(    100, FULL,    100baseT_Full		),
257 	PHY_SETTING(    100, FULL,    100baseT1_Full		),
258 	PHY_SETTING(    100, HALF,    100baseT_Half		),
259 	PHY_SETTING(    100, HALF,    100baseFX_Half		),
260 	PHY_SETTING(    100, FULL,    100baseFX_Full		),
261 	/* 10M */
262 	PHY_SETTING(     10, FULL,     10baseT_Full		),
263 	PHY_SETTING(     10, HALF,     10baseT_Half		),
264 	PHY_SETTING(     10, FULL,     10baseT1L_Full		),
265 	PHY_SETTING(     10, FULL,     10baseT1S_Full		),
266 	PHY_SETTING(     10, HALF,     10baseT1S_Half		),
267 	PHY_SETTING(     10, HALF,     10baseT1S_P2MP_Half	),
268 };
269 #undef PHY_SETTING
270 
271 /**
272  * phy_lookup_setting - lookup a PHY setting
273  * @speed: speed to match
274  * @duplex: duplex to match
275  * @mask: allowed link modes
276  * @exact: an exact match is required
277  *
278  * Search the settings array for a setting that matches the speed and
279  * duplex, and which is supported.
280  *
281  * If @exact is unset, either an exact match or %NULL for no match will
282  * be returned.
283  *
284  * If @exact is set, an exact match, the fastest supported setting at
285  * or below the specified speed, the slowest supported setting, or if
286  * they all fail, %NULL will be returned.
287  */
288 const struct phy_setting *
phy_lookup_setting(int speed,int duplex,const unsigned long * mask,bool exact)289 phy_lookup_setting(int speed, int duplex, const unsigned long *mask, bool exact)
290 {
291 	const struct phy_setting *p, *match = NULL, *last = NULL;
292 	int i;
293 
294 	for (i = 0, p = settings; i < ARRAY_SIZE(settings); i++, p++) {
295 		if (p->bit < __ETHTOOL_LINK_MODE_MASK_NBITS &&
296 		    test_bit(p->bit, mask)) {
297 			last = p;
298 			if (p->speed == speed && p->duplex == duplex) {
299 				/* Exact match for speed and duplex */
300 				match = p;
301 				break;
302 			} else if (!exact) {
303 				if (!match && p->speed <= speed)
304 					/* Candidate */
305 					match = p;
306 
307 				if (p->speed < speed)
308 					break;
309 			}
310 		}
311 	}
312 
313 	if (!match && !exact)
314 		match = last;
315 
316 	return match;
317 }
318 EXPORT_SYMBOL_GPL(phy_lookup_setting);
319 
phy_speeds(unsigned int * speeds,size_t size,unsigned long * mask)320 size_t phy_speeds(unsigned int *speeds, size_t size,
321 		  unsigned long *mask)
322 {
323 	size_t count;
324 	int i;
325 
326 	for (i = 0, count = 0; i < ARRAY_SIZE(settings) && count < size; i++)
327 		if (settings[i].bit < __ETHTOOL_LINK_MODE_MASK_NBITS &&
328 		    test_bit(settings[i].bit, mask) &&
329 		    (count == 0 || speeds[count - 1] != settings[i].speed))
330 			speeds[count++] = settings[i].speed;
331 
332 	return count;
333 }
334 
__set_linkmode_max_speed(u32 max_speed,unsigned long * addr)335 static void __set_linkmode_max_speed(u32 max_speed, unsigned long *addr)
336 {
337 	const struct phy_setting *p;
338 	int i;
339 
340 	for (i = 0, p = settings; i < ARRAY_SIZE(settings); i++, p++) {
341 		if (p->speed > max_speed)
342 			linkmode_clear_bit(p->bit, addr);
343 		else
344 			break;
345 	}
346 }
347 
__set_phy_supported(struct phy_device * phydev,u32 max_speed)348 static void __set_phy_supported(struct phy_device *phydev, u32 max_speed)
349 {
350 	__set_linkmode_max_speed(max_speed, phydev->supported);
351 }
352 
353 /**
354  * phy_set_max_speed - Set the maximum speed the PHY should support
355  *
356  * @phydev: The phy_device struct
357  * @max_speed: Maximum speed
358  *
359  * The PHY might be more capable than the MAC. For example a Fast Ethernet
360  * is connected to a 1G PHY. This function allows the MAC to indicate its
361  * maximum speed, and so limit what the PHY will advertise.
362  */
phy_set_max_speed(struct phy_device * phydev,u32 max_speed)363 void phy_set_max_speed(struct phy_device *phydev, u32 max_speed)
364 {
365 	__set_phy_supported(phydev, max_speed);
366 
367 	phy_advertise_supported(phydev);
368 }
369 EXPORT_SYMBOL(phy_set_max_speed);
370 
of_set_phy_supported(struct phy_device * phydev)371 void of_set_phy_supported(struct phy_device *phydev)
372 {
373 	struct device_node *node = phydev->mdio.dev.of_node;
374 	u32 max_speed;
375 
376 	if (!IS_ENABLED(CONFIG_OF_MDIO))
377 		return;
378 
379 	if (!node)
380 		return;
381 
382 	if (!of_property_read_u32(node, "max-speed", &max_speed))
383 		__set_phy_supported(phydev, max_speed);
384 }
385 
of_set_phy_eee_broken(struct phy_device * phydev)386 void of_set_phy_eee_broken(struct phy_device *phydev)
387 {
388 	struct device_node *node = phydev->mdio.dev.of_node;
389 	u32 broken = 0;
390 
391 	if (!IS_ENABLED(CONFIG_OF_MDIO))
392 		return;
393 
394 	if (!node)
395 		return;
396 
397 	if (of_property_read_bool(node, "eee-broken-100tx"))
398 		broken |= MDIO_EEE_100TX;
399 	if (of_property_read_bool(node, "eee-broken-1000t"))
400 		broken |= MDIO_EEE_1000T;
401 	if (of_property_read_bool(node, "eee-broken-10gt"))
402 		broken |= MDIO_EEE_10GT;
403 	if (of_property_read_bool(node, "eee-broken-1000kx"))
404 		broken |= MDIO_EEE_1000KX;
405 	if (of_property_read_bool(node, "eee-broken-10gkx4"))
406 		broken |= MDIO_EEE_10GKX4;
407 	if (of_property_read_bool(node, "eee-broken-10gkr"))
408 		broken |= MDIO_EEE_10GKR;
409 
410 	phydev->eee_broken_modes = broken;
411 }
412 
413 /**
414  * phy_resolve_aneg_pause - Determine pause autoneg results
415  *
416  * @phydev: The phy_device struct
417  *
418  * Once autoneg has completed the local pause settings can be
419  * resolved.  Determine if pause and asymmetric pause should be used
420  * by the MAC.
421  */
422 
phy_resolve_aneg_pause(struct phy_device * phydev)423 void phy_resolve_aneg_pause(struct phy_device *phydev)
424 {
425 	if (phydev->duplex == DUPLEX_FULL) {
426 		phydev->pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
427 						  phydev->lp_advertising);
428 		phydev->asym_pause = linkmode_test_bit(
429 			ETHTOOL_LINK_MODE_Asym_Pause_BIT,
430 			phydev->lp_advertising);
431 	}
432 }
433 EXPORT_SYMBOL_GPL(phy_resolve_aneg_pause);
434 
435 /**
436  * phy_resolve_aneg_linkmode - resolve the advertisements into PHY settings
437  * @phydev: The phy_device struct
438  *
439  * Resolve our and the link partner advertisements into their corresponding
440  * speed and duplex. If full duplex was negotiated, extract the pause mode
441  * from the link partner mask.
442  */
phy_resolve_aneg_linkmode(struct phy_device * phydev)443 void phy_resolve_aneg_linkmode(struct phy_device *phydev)
444 {
445 	__ETHTOOL_DECLARE_LINK_MODE_MASK(common);
446 	int i;
447 
448 	linkmode_and(common, phydev->lp_advertising, phydev->advertising);
449 
450 	for (i = 0; i < ARRAY_SIZE(settings); i++)
451 		if (test_bit(settings[i].bit, common)) {
452 			phydev->speed = settings[i].speed;
453 			phydev->duplex = settings[i].duplex;
454 			break;
455 		}
456 
457 	phy_resolve_aneg_pause(phydev);
458 }
459 EXPORT_SYMBOL_GPL(phy_resolve_aneg_linkmode);
460 
461 /**
462  * phy_check_downshift - check whether downshift occurred
463  * @phydev: The phy_device struct
464  *
465  * Check whether a downshift to a lower speed occurred. If this should be the
466  * case warn the user.
467  * Prerequisite for detecting downshift is that PHY driver implements the
468  * read_status callback and sets phydev->speed to the actual link speed.
469  */
phy_check_downshift(struct phy_device * phydev)470 void phy_check_downshift(struct phy_device *phydev)
471 {
472 	__ETHTOOL_DECLARE_LINK_MODE_MASK(common);
473 	int i, speed = SPEED_UNKNOWN;
474 
475 	phydev->downshifted_rate = 0;
476 
477 	if (phydev->autoneg == AUTONEG_DISABLE ||
478 	    phydev->speed == SPEED_UNKNOWN)
479 		return;
480 
481 	linkmode_and(common, phydev->lp_advertising, phydev->advertising);
482 
483 	for (i = 0; i < ARRAY_SIZE(settings); i++)
484 		if (test_bit(settings[i].bit, common)) {
485 			speed = settings[i].speed;
486 			break;
487 		}
488 
489 	if (speed == SPEED_UNKNOWN || phydev->speed >= speed)
490 		return;
491 
492 	phydev_warn(phydev, "Downshift occurred from negotiated speed %s to actual speed %s, check cabling!\n",
493 		    phy_speed_to_str(speed), phy_speed_to_str(phydev->speed));
494 
495 	phydev->downshifted_rate = 1;
496 }
497 EXPORT_SYMBOL_GPL(phy_check_downshift);
498 
phy_resolve_min_speed(struct phy_device * phydev,bool fdx_only)499 static int phy_resolve_min_speed(struct phy_device *phydev, bool fdx_only)
500 {
501 	__ETHTOOL_DECLARE_LINK_MODE_MASK(common);
502 	int i = ARRAY_SIZE(settings);
503 
504 	linkmode_and(common, phydev->lp_advertising, phydev->advertising);
505 
506 	while (--i >= 0) {
507 		if (test_bit(settings[i].bit, common)) {
508 			if (fdx_only && settings[i].duplex != DUPLEX_FULL)
509 				continue;
510 			return settings[i].speed;
511 		}
512 	}
513 
514 	return SPEED_UNKNOWN;
515 }
516 
phy_speed_down_core(struct phy_device * phydev)517 int phy_speed_down_core(struct phy_device *phydev)
518 {
519 	int min_common_speed = phy_resolve_min_speed(phydev, true);
520 
521 	if (min_common_speed == SPEED_UNKNOWN)
522 		return -EINVAL;
523 
524 	__set_linkmode_max_speed(min_common_speed, phydev->advertising);
525 
526 	return 0;
527 }
528 
mmd_phy_indirect(struct mii_bus * bus,int phy_addr,int devad,u16 regnum)529 static void mmd_phy_indirect(struct mii_bus *bus, int phy_addr, int devad,
530 			     u16 regnum)
531 {
532 	/* Write the desired MMD Devad */
533 	__mdiobus_write(bus, phy_addr, MII_MMD_CTRL, devad);
534 
535 	/* Write the desired MMD register address */
536 	__mdiobus_write(bus, phy_addr, MII_MMD_DATA, regnum);
537 
538 	/* Select the Function : DATA with no post increment */
539 	__mdiobus_write(bus, phy_addr, MII_MMD_CTRL,
540 			devad | MII_MMD_CTRL_NOINCR);
541 }
542 
543 /**
544  * __phy_read_mmd - Convenience function for reading a register
545  * from an MMD on a given PHY.
546  * @phydev: The phy_device struct
547  * @devad: The MMD to read from (0..31)
548  * @regnum: The register on the MMD to read (0..65535)
549  *
550  * Same rules as for __phy_read();
551  */
__phy_read_mmd(struct phy_device * phydev,int devad,u32 regnum)552 int __phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
553 {
554 	int val;
555 
556 	if (regnum > (u16)~0 || devad > 32)
557 		return -EINVAL;
558 
559 	if (phydev->drv && phydev->drv->read_mmd) {
560 		val = phydev->drv->read_mmd(phydev, devad, regnum);
561 	} else if (phydev->is_c45) {
562 		val = __mdiobus_c45_read(phydev->mdio.bus, phydev->mdio.addr,
563 					 devad, regnum);
564 	} else {
565 		struct mii_bus *bus = phydev->mdio.bus;
566 		int phy_addr = phydev->mdio.addr;
567 
568 		mmd_phy_indirect(bus, phy_addr, devad, regnum);
569 
570 		/* Read the content of the MMD's selected register */
571 		val = __mdiobus_read(bus, phy_addr, MII_MMD_DATA);
572 	}
573 	return val;
574 }
575 EXPORT_SYMBOL(__phy_read_mmd);
576 
577 /**
578  * phy_read_mmd - Convenience function for reading a register
579  * from an MMD on a given PHY.
580  * @phydev: The phy_device struct
581  * @devad: The MMD to read from
582  * @regnum: The register on the MMD to read
583  *
584  * Same rules as for phy_read();
585  */
phy_read_mmd(struct phy_device * phydev,int devad,u32 regnum)586 int phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
587 {
588 	int ret;
589 
590 	phy_lock_mdio_bus(phydev);
591 	ret = __phy_read_mmd(phydev, devad, regnum);
592 	phy_unlock_mdio_bus(phydev);
593 
594 	return ret;
595 }
596 EXPORT_SYMBOL(phy_read_mmd);
597 
598 /**
599  * __phy_write_mmd - Convenience function for writing a register
600  * on an MMD on a given PHY.
601  * @phydev: The phy_device struct
602  * @devad: The MMD to read from
603  * @regnum: The register on the MMD to read
604  * @val: value to write to @regnum
605  *
606  * Same rules as for __phy_write();
607  */
__phy_write_mmd(struct phy_device * phydev,int devad,u32 regnum,u16 val)608 int __phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
609 {
610 	int ret;
611 
612 	if (regnum > (u16)~0 || devad > 32)
613 		return -EINVAL;
614 
615 	if (phydev->drv && phydev->drv->write_mmd) {
616 		ret = phydev->drv->write_mmd(phydev, devad, regnum, val);
617 	} else if (phydev->is_c45) {
618 		ret = __mdiobus_c45_write(phydev->mdio.bus, phydev->mdio.addr,
619 					  devad, regnum, val);
620 	} else {
621 		struct mii_bus *bus = phydev->mdio.bus;
622 		int phy_addr = phydev->mdio.addr;
623 
624 		mmd_phy_indirect(bus, phy_addr, devad, regnum);
625 
626 		/* Write the data into MMD's selected register */
627 		__mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
628 
629 		ret = 0;
630 	}
631 	return ret;
632 }
633 EXPORT_SYMBOL(__phy_write_mmd);
634 
635 /**
636  * phy_write_mmd - Convenience function for writing a register
637  * on an MMD on a given PHY.
638  * @phydev: The phy_device struct
639  * @devad: The MMD to read from
640  * @regnum: The register on the MMD to read
641  * @val: value to write to @regnum
642  *
643  * Same rules as for phy_write();
644  */
phy_write_mmd(struct phy_device * phydev,int devad,u32 regnum,u16 val)645 int phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
646 {
647 	int ret;
648 
649 	phy_lock_mdio_bus(phydev);
650 	ret = __phy_write_mmd(phydev, devad, regnum, val);
651 	phy_unlock_mdio_bus(phydev);
652 
653 	return ret;
654 }
655 EXPORT_SYMBOL(phy_write_mmd);
656 
657 /**
658  * phy_modify_changed - Function for modifying a PHY register
659  * @phydev: the phy_device struct
660  * @regnum: register number to modify
661  * @mask: bit mask of bits to clear
662  * @set: new value of bits set in mask to write to @regnum
663  *
664  * NOTE: MUST NOT be called from interrupt context,
665  * because the bus read/write functions may wait for an interrupt
666  * to conclude the operation.
667  *
668  * Returns negative errno, 0 if there was no change, and 1 in case of change
669  */
phy_modify_changed(struct phy_device * phydev,u32 regnum,u16 mask,u16 set)670 int phy_modify_changed(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
671 {
672 	int ret;
673 
674 	phy_lock_mdio_bus(phydev);
675 	ret = __phy_modify_changed(phydev, regnum, mask, set);
676 	phy_unlock_mdio_bus(phydev);
677 
678 	return ret;
679 }
680 EXPORT_SYMBOL_GPL(phy_modify_changed);
681 
682 /**
683  * __phy_modify - Convenience function for modifying a PHY register
684  * @phydev: the phy_device struct
685  * @regnum: register number to modify
686  * @mask: bit mask of bits to clear
687  * @set: new value of bits set in mask to write to @regnum
688  *
689  * NOTE: MUST NOT be called from interrupt context,
690  * because the bus read/write functions may wait for an interrupt
691  * to conclude the operation.
692  */
__phy_modify(struct phy_device * phydev,u32 regnum,u16 mask,u16 set)693 int __phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
694 {
695 	int ret;
696 
697 	ret = __phy_modify_changed(phydev, regnum, mask, set);
698 
699 	return ret < 0 ? ret : 0;
700 }
701 EXPORT_SYMBOL_GPL(__phy_modify);
702 
703 /**
704  * phy_modify - Convenience function for modifying a given PHY register
705  * @phydev: the phy_device struct
706  * @regnum: register number to write
707  * @mask: bit mask of bits to clear
708  * @set: new value of bits set in mask to write to @regnum
709  *
710  * NOTE: MUST NOT be called from interrupt context,
711  * because the bus read/write functions may wait for an interrupt
712  * to conclude the operation.
713  */
phy_modify(struct phy_device * phydev,u32 regnum,u16 mask,u16 set)714 int phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
715 {
716 	int ret;
717 
718 	phy_lock_mdio_bus(phydev);
719 	ret = __phy_modify(phydev, regnum, mask, set);
720 	phy_unlock_mdio_bus(phydev);
721 
722 	return ret;
723 }
724 EXPORT_SYMBOL_GPL(phy_modify);
725 
726 /**
727  * __phy_modify_mmd_changed - Function for modifying a register on MMD
728  * @phydev: the phy_device struct
729  * @devad: the MMD containing register to modify
730  * @regnum: register number to modify
731  * @mask: bit mask of bits to clear
732  * @set: new value of bits set in mask to write to @regnum
733  *
734  * Unlocked helper function which allows a MMD register to be modified as
735  * new register value = (old register value & ~mask) | set
736  *
737  * Returns negative errno, 0 if there was no change, and 1 in case of change
738  */
__phy_modify_mmd_changed(struct phy_device * phydev,int devad,u32 regnum,u16 mask,u16 set)739 int __phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
740 			     u16 mask, u16 set)
741 {
742 	int new, ret;
743 
744 	ret = __phy_read_mmd(phydev, devad, regnum);
745 	if (ret < 0)
746 		return ret;
747 
748 	new = (ret & ~mask) | set;
749 	if (new == ret)
750 		return 0;
751 
752 	ret = __phy_write_mmd(phydev, devad, regnum, new);
753 
754 	return ret < 0 ? ret : 1;
755 }
756 EXPORT_SYMBOL_GPL(__phy_modify_mmd_changed);
757 
758 /**
759  * phy_modify_mmd_changed - Function for modifying a register on MMD
760  * @phydev: the phy_device struct
761  * @devad: the MMD containing register to modify
762  * @regnum: register number to modify
763  * @mask: bit mask of bits to clear
764  * @set: new value of bits set in mask to write to @regnum
765  *
766  * NOTE: MUST NOT be called from interrupt context,
767  * because the bus read/write functions may wait for an interrupt
768  * to conclude the operation.
769  *
770  * Returns negative errno, 0 if there was no change, and 1 in case of change
771  */
phy_modify_mmd_changed(struct phy_device * phydev,int devad,u32 regnum,u16 mask,u16 set)772 int phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
773 			   u16 mask, u16 set)
774 {
775 	int ret;
776 
777 	phy_lock_mdio_bus(phydev);
778 	ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
779 	phy_unlock_mdio_bus(phydev);
780 
781 	return ret;
782 }
783 EXPORT_SYMBOL_GPL(phy_modify_mmd_changed);
784 
785 /**
786  * __phy_modify_mmd - Convenience function for modifying a register on MMD
787  * @phydev: the phy_device struct
788  * @devad: the MMD containing register to modify
789  * @regnum: register number to modify
790  * @mask: bit mask of bits to clear
791  * @set: new value of bits set in mask to write to @regnum
792  *
793  * NOTE: MUST NOT be called from interrupt context,
794  * because the bus read/write functions may wait for an interrupt
795  * to conclude the operation.
796  */
__phy_modify_mmd(struct phy_device * phydev,int devad,u32 regnum,u16 mask,u16 set)797 int __phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
798 		     u16 mask, u16 set)
799 {
800 	int ret;
801 
802 	ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
803 
804 	return ret < 0 ? ret : 0;
805 }
806 EXPORT_SYMBOL_GPL(__phy_modify_mmd);
807 
808 /**
809  * phy_modify_mmd - Convenience function for modifying a register on MMD
810  * @phydev: the phy_device struct
811  * @devad: the MMD containing register to modify
812  * @regnum: register number to modify
813  * @mask: bit mask of bits to clear
814  * @set: new value of bits set in mask to write to @regnum
815  *
816  * NOTE: MUST NOT be called from interrupt context,
817  * because the bus read/write functions may wait for an interrupt
818  * to conclude the operation.
819  */
phy_modify_mmd(struct phy_device * phydev,int devad,u32 regnum,u16 mask,u16 set)820 int phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
821 		   u16 mask, u16 set)
822 {
823 	int ret;
824 
825 	phy_lock_mdio_bus(phydev);
826 	ret = __phy_modify_mmd(phydev, devad, regnum, mask, set);
827 	phy_unlock_mdio_bus(phydev);
828 
829 	return ret;
830 }
831 EXPORT_SYMBOL_GPL(phy_modify_mmd);
832 
__phy_read_page(struct phy_device * phydev)833 static int __phy_read_page(struct phy_device *phydev)
834 {
835 	if (WARN_ONCE(!phydev->drv->read_page, "read_page callback not available, PHY driver not loaded?\n"))
836 		return -EOPNOTSUPP;
837 
838 	return phydev->drv->read_page(phydev);
839 }
840 
__phy_write_page(struct phy_device * phydev,int page)841 static int __phy_write_page(struct phy_device *phydev, int page)
842 {
843 	if (WARN_ONCE(!phydev->drv->write_page, "write_page callback not available, PHY driver not loaded?\n"))
844 		return -EOPNOTSUPP;
845 
846 	return phydev->drv->write_page(phydev, page);
847 }
848 
849 /**
850  * phy_save_page() - take the bus lock and save the current page
851  * @phydev: a pointer to a &struct phy_device
852  *
853  * Take the MDIO bus lock, and return the current page number. On error,
854  * returns a negative errno. phy_restore_page() must always be called
855  * after this, irrespective of success or failure of this call.
856  */
phy_save_page(struct phy_device * phydev)857 int phy_save_page(struct phy_device *phydev)
858 {
859 	phy_lock_mdio_bus(phydev);
860 	return __phy_read_page(phydev);
861 }
862 EXPORT_SYMBOL_GPL(phy_save_page);
863 
864 /**
865  * phy_select_page() - take the bus lock, save the current page, and set a page
866  * @phydev: a pointer to a &struct phy_device
867  * @page: desired page
868  *
869  * Take the MDIO bus lock to protect against concurrent access, save the
870  * current PHY page, and set the current page.  On error, returns a
871  * negative errno, otherwise returns the previous page number.
872  * phy_restore_page() must always be called after this, irrespective
873  * of success or failure of this call.
874  */
phy_select_page(struct phy_device * phydev,int page)875 int phy_select_page(struct phy_device *phydev, int page)
876 {
877 	int ret, oldpage;
878 
879 	oldpage = ret = phy_save_page(phydev);
880 	if (ret < 0)
881 		return ret;
882 
883 	if (oldpage != page) {
884 		ret = __phy_write_page(phydev, page);
885 		if (ret < 0)
886 			return ret;
887 	}
888 
889 	return oldpage;
890 }
891 EXPORT_SYMBOL_GPL(phy_select_page);
892 
893 /**
894  * phy_restore_page() - restore the page register and release the bus lock
895  * @phydev: a pointer to a &struct phy_device
896  * @oldpage: the old page, return value from phy_save_page() or phy_select_page()
897  * @ret: operation's return code
898  *
899  * Release the MDIO bus lock, restoring @oldpage if it is a valid page.
900  * This function propagates the earliest error code from the group of
901  * operations.
902  *
903  * Returns:
904  *   @oldpage if it was a negative value, otherwise
905  *   @ret if it was a negative errno value, otherwise
906  *   phy_write_page()'s negative value if it were in error, otherwise
907  *   @ret.
908  */
phy_restore_page(struct phy_device * phydev,int oldpage,int ret)909 int phy_restore_page(struct phy_device *phydev, int oldpage, int ret)
910 {
911 	int r;
912 
913 	if (oldpage >= 0) {
914 		r = __phy_write_page(phydev, oldpage);
915 
916 		/* Propagate the operation return code if the page write
917 		 * was successful.
918 		 */
919 		if (ret >= 0 && r < 0)
920 			ret = r;
921 	} else {
922 		/* Propagate the phy page selection error code */
923 		ret = oldpage;
924 	}
925 
926 	phy_unlock_mdio_bus(phydev);
927 
928 	return ret;
929 }
930 EXPORT_SYMBOL_GPL(phy_restore_page);
931 
932 /**
933  * phy_read_paged() - Convenience function for reading a paged register
934  * @phydev: a pointer to a &struct phy_device
935  * @page: the page for the phy
936  * @regnum: register number
937  *
938  * Same rules as for phy_read().
939  */
phy_read_paged(struct phy_device * phydev,int page,u32 regnum)940 int phy_read_paged(struct phy_device *phydev, int page, u32 regnum)
941 {
942 	int ret = 0, oldpage;
943 
944 	oldpage = phy_select_page(phydev, page);
945 	if (oldpage >= 0)
946 		ret = __phy_read(phydev, regnum);
947 
948 	return phy_restore_page(phydev, oldpage, ret);
949 }
950 EXPORT_SYMBOL(phy_read_paged);
951 
952 /**
953  * phy_write_paged() - Convenience function for writing a paged register
954  * @phydev: a pointer to a &struct phy_device
955  * @page: the page for the phy
956  * @regnum: register number
957  * @val: value to write
958  *
959  * Same rules as for phy_write().
960  */
phy_write_paged(struct phy_device * phydev,int page,u32 regnum,u16 val)961 int phy_write_paged(struct phy_device *phydev, int page, u32 regnum, u16 val)
962 {
963 	int ret = 0, oldpage;
964 
965 	oldpage = phy_select_page(phydev, page);
966 	if (oldpage >= 0)
967 		ret = __phy_write(phydev, regnum, val);
968 
969 	return phy_restore_page(phydev, oldpage, ret);
970 }
971 EXPORT_SYMBOL(phy_write_paged);
972 
973 /**
974  * phy_modify_paged_changed() - Function for modifying a paged register
975  * @phydev: a pointer to a &struct phy_device
976  * @page: the page for the phy
977  * @regnum: register number
978  * @mask: bit mask of bits to clear
979  * @set: bit mask of bits to set
980  *
981  * Returns negative errno, 0 if there was no change, and 1 in case of change
982  */
phy_modify_paged_changed(struct phy_device * phydev,int page,u32 regnum,u16 mask,u16 set)983 int phy_modify_paged_changed(struct phy_device *phydev, int page, u32 regnum,
984 			     u16 mask, u16 set)
985 {
986 	int ret = 0, oldpage;
987 
988 	oldpage = phy_select_page(phydev, page);
989 	if (oldpage >= 0)
990 		ret = __phy_modify_changed(phydev, regnum, mask, set);
991 
992 	return phy_restore_page(phydev, oldpage, ret);
993 }
994 EXPORT_SYMBOL(phy_modify_paged_changed);
995 
996 /**
997  * phy_modify_paged() - Convenience function for modifying a paged register
998  * @phydev: a pointer to a &struct phy_device
999  * @page: the page for the phy
1000  * @regnum: register number
1001  * @mask: bit mask of bits to clear
1002  * @set: bit mask of bits to set
1003  *
1004  * Same rules as for phy_read() and phy_write().
1005  */
phy_modify_paged(struct phy_device * phydev,int page,u32 regnum,u16 mask,u16 set)1006 int phy_modify_paged(struct phy_device *phydev, int page, u32 regnum,
1007 		     u16 mask, u16 set)
1008 {
1009 	int ret = phy_modify_paged_changed(phydev, page, regnum, mask, set);
1010 
1011 	return ret < 0 ? ret : 0;
1012 }
1013 EXPORT_SYMBOL(phy_modify_paged);
1014