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