xref: /openbmc/linux/drivers/net/phy/phy_device.c (revision b6dcefde)
1 /*
2  * drivers/net/phy/phy_device.c
3  *
4  * Framework for finding and configuring PHYs.
5  * Also contains generic PHY driver
6  *
7  * Author: Andy Fleming
8  *
9  * Copyright (c) 2004 Freescale Semiconductor, Inc.
10  *
11  * This program is free software; you can redistribute  it and/or modify it
12  * under  the terms of  the GNU General  Public License as published by the
13  * Free Software Foundation;  either version 2 of the  License, or (at your
14  * option) any later version.
15  *
16  */
17 #include <linux/kernel.h>
18 #include <linux/string.h>
19 #include <linux/errno.h>
20 #include <linux/unistd.h>
21 #include <linux/slab.h>
22 #include <linux/interrupt.h>
23 #include <linux/init.h>
24 #include <linux/delay.h>
25 #include <linux/netdevice.h>
26 #include <linux/etherdevice.h>
27 #include <linux/skbuff.h>
28 #include <linux/mm.h>
29 #include <linux/module.h>
30 #include <linux/mii.h>
31 #include <linux/ethtool.h>
32 #include <linux/phy.h>
33 
34 #include <asm/io.h>
35 #include <asm/irq.h>
36 #include <asm/uaccess.h>
37 
38 MODULE_DESCRIPTION("PHY library");
39 MODULE_AUTHOR("Andy Fleming");
40 MODULE_LICENSE("GPL");
41 
42 void phy_device_free(struct phy_device *phydev)
43 {
44 	kfree(phydev);
45 }
46 EXPORT_SYMBOL(phy_device_free);
47 
48 static void phy_device_release(struct device *dev)
49 {
50 	phy_device_free(to_phy_device(dev));
51 }
52 
53 static struct phy_driver genphy_driver;
54 extern int mdio_bus_init(void);
55 extern void mdio_bus_exit(void);
56 
57 static LIST_HEAD(phy_fixup_list);
58 static DEFINE_MUTEX(phy_fixup_lock);
59 
60 /*
61  * Creates a new phy_fixup and adds it to the list
62  * @bus_id: A string which matches phydev->dev.bus_id (or PHY_ANY_ID)
63  * @phy_uid: Used to match against phydev->phy_id (the UID of the PHY)
64  * 	It can also be PHY_ANY_UID
65  * @phy_uid_mask: Applied to phydev->phy_id and fixup->phy_uid before
66  * 	comparison
67  * @run: The actual code to be run when a matching PHY is found
68  */
69 int phy_register_fixup(const char *bus_id, u32 phy_uid, u32 phy_uid_mask,
70 		int (*run)(struct phy_device *))
71 {
72 	struct phy_fixup *fixup;
73 
74 	fixup = kzalloc(sizeof(struct phy_fixup), GFP_KERNEL);
75 	if (!fixup)
76 		return -ENOMEM;
77 
78 	strlcpy(fixup->bus_id, bus_id, sizeof(fixup->bus_id));
79 	fixup->phy_uid = phy_uid;
80 	fixup->phy_uid_mask = phy_uid_mask;
81 	fixup->run = run;
82 
83 	mutex_lock(&phy_fixup_lock);
84 	list_add_tail(&fixup->list, &phy_fixup_list);
85 	mutex_unlock(&phy_fixup_lock);
86 
87 	return 0;
88 }
89 EXPORT_SYMBOL(phy_register_fixup);
90 
91 /* Registers a fixup to be run on any PHY with the UID in phy_uid */
92 int phy_register_fixup_for_uid(u32 phy_uid, u32 phy_uid_mask,
93 		int (*run)(struct phy_device *))
94 {
95 	return phy_register_fixup(PHY_ANY_ID, phy_uid, phy_uid_mask, run);
96 }
97 EXPORT_SYMBOL(phy_register_fixup_for_uid);
98 
99 /* Registers a fixup to be run on the PHY with id string bus_id */
100 int phy_register_fixup_for_id(const char *bus_id,
101 		int (*run)(struct phy_device *))
102 {
103 	return phy_register_fixup(bus_id, PHY_ANY_UID, 0xffffffff, run);
104 }
105 EXPORT_SYMBOL(phy_register_fixup_for_id);
106 
107 /*
108  * Returns 1 if fixup matches phydev in bus_id and phy_uid.
109  * Fixups can be set to match any in one or more fields.
110  */
111 static int phy_needs_fixup(struct phy_device *phydev, struct phy_fixup *fixup)
112 {
113 	if (strcmp(fixup->bus_id, dev_name(&phydev->dev)) != 0)
114 		if (strcmp(fixup->bus_id, PHY_ANY_ID) != 0)
115 			return 0;
116 
117 	if ((fixup->phy_uid & fixup->phy_uid_mask) !=
118 			(phydev->phy_id & fixup->phy_uid_mask))
119 		if (fixup->phy_uid != PHY_ANY_UID)
120 			return 0;
121 
122 	return 1;
123 }
124 
125 /* Runs any matching fixups for this phydev */
126 int phy_scan_fixups(struct phy_device *phydev)
127 {
128 	struct phy_fixup *fixup;
129 
130 	mutex_lock(&phy_fixup_lock);
131 	list_for_each_entry(fixup, &phy_fixup_list, list) {
132 		if (phy_needs_fixup(phydev, fixup)) {
133 			int err;
134 
135 			err = fixup->run(phydev);
136 
137 			if (err < 0) {
138 				mutex_unlock(&phy_fixup_lock);
139 				return err;
140 			}
141 		}
142 	}
143 	mutex_unlock(&phy_fixup_lock);
144 
145 	return 0;
146 }
147 EXPORT_SYMBOL(phy_scan_fixups);
148 
149 struct phy_device* phy_device_create(struct mii_bus *bus, int addr, int phy_id)
150 {
151 	struct phy_device *dev;
152 	/* We allocate the device, and initialize the
153 	 * default values */
154 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
155 
156 	if (NULL == dev)
157 		return (struct phy_device*) PTR_ERR((void*)-ENOMEM);
158 
159 	dev->dev.release = phy_device_release;
160 
161 	dev->speed = 0;
162 	dev->duplex = -1;
163 	dev->pause = dev->asym_pause = 0;
164 	dev->link = 1;
165 	dev->interface = PHY_INTERFACE_MODE_GMII;
166 
167 	dev->autoneg = AUTONEG_ENABLE;
168 
169 	dev->addr = addr;
170 	dev->phy_id = phy_id;
171 	dev->bus = bus;
172 	dev->dev.parent = bus->parent;
173 	dev->dev.bus = &mdio_bus_type;
174 	dev->irq = bus->irq != NULL ? bus->irq[addr] : PHY_POLL;
175 	dev_set_name(&dev->dev, PHY_ID_FMT, bus->id, addr);
176 
177 	dev->state = PHY_DOWN;
178 
179 	mutex_init(&dev->lock);
180 	INIT_DELAYED_WORK(&dev->state_queue, phy_state_machine);
181 
182 	return dev;
183 }
184 EXPORT_SYMBOL(phy_device_create);
185 
186 /**
187  * get_phy_id - reads the specified addr for its ID.
188  * @bus: the target MII bus
189  * @addr: PHY address on the MII bus
190  * @phy_id: where to store the ID retrieved.
191  *
192  * Description: Reads the ID registers of the PHY at @addr on the
193  *   @bus, stores it in @phy_id and returns zero on success.
194  */
195 int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
196 {
197 	int phy_reg;
198 
199 	/* Grab the bits from PHYIR1, and put them
200 	 * in the upper half */
201 	phy_reg = bus->read(bus, addr, MII_PHYSID1);
202 
203 	if (phy_reg < 0)
204 		return -EIO;
205 
206 	*phy_id = (phy_reg & 0xffff) << 16;
207 
208 	/* Grab the bits from PHYIR2, and put them in the lower half */
209 	phy_reg = bus->read(bus, addr, MII_PHYSID2);
210 
211 	if (phy_reg < 0)
212 		return -EIO;
213 
214 	*phy_id |= (phy_reg & 0xffff);
215 
216 	return 0;
217 }
218 EXPORT_SYMBOL(get_phy_id);
219 
220 /**
221  * get_phy_device - reads the specified PHY device and returns its @phy_device struct
222  * @bus: the target MII bus
223  * @addr: PHY address on the MII bus
224  *
225  * Description: Reads the ID registers of the PHY at @addr on the
226  *   @bus, then allocates and returns the phy_device to represent it.
227  */
228 struct phy_device * get_phy_device(struct mii_bus *bus, int addr)
229 {
230 	struct phy_device *dev = NULL;
231 	u32 phy_id;
232 	int r;
233 
234 	r = get_phy_id(bus, addr, &phy_id);
235 	if (r)
236 		return ERR_PTR(r);
237 
238 	/* If the phy_id is mostly Fs, there is no device there */
239 	if ((phy_id & 0x1fffffff) == 0x1fffffff)
240 		return NULL;
241 
242 	dev = phy_device_create(bus, addr, phy_id);
243 
244 	return dev;
245 }
246 EXPORT_SYMBOL(get_phy_device);
247 
248 /**
249  * phy_device_register - Register the phy device on the MDIO bus
250  * @phydev: phy_device structure to be added to the MDIO bus
251  */
252 int phy_device_register(struct phy_device *phydev)
253 {
254 	int err;
255 
256 	/* Don't register a phy if one is already registered at this
257 	 * address */
258 	if (phydev->bus->phy_map[phydev->addr])
259 		return -EINVAL;
260 	phydev->bus->phy_map[phydev->addr] = phydev;
261 
262 	/* Run all of the fixups for this PHY */
263 	phy_scan_fixups(phydev);
264 
265 	err = device_register(&phydev->dev);
266 	if (err) {
267 		pr_err("phy %d failed to register\n", phydev->addr);
268 		goto out;
269 	}
270 
271 	return 0;
272 
273  out:
274 	phydev->bus->phy_map[phydev->addr] = NULL;
275 	return err;
276 }
277 EXPORT_SYMBOL(phy_device_register);
278 
279 /**
280  * phy_prepare_link - prepares the PHY layer to monitor link status
281  * @phydev: target phy_device struct
282  * @handler: callback function for link status change notifications
283  *
284  * Description: Tells the PHY infrastructure to handle the
285  *   gory details on monitoring link status (whether through
286  *   polling or an interrupt), and to call back to the
287  *   connected device driver when the link status changes.
288  *   If you want to monitor your own link state, don't call
289  *   this function.
290  */
291 void phy_prepare_link(struct phy_device *phydev,
292 		void (*handler)(struct net_device *))
293 {
294 	phydev->adjust_link = handler;
295 }
296 
297 /**
298  * phy_connect_direct - connect an ethernet device to a specific phy_device
299  * @dev: the network device to connect
300  * @phydev: the pointer to the phy device
301  * @handler: callback function for state change notifications
302  * @flags: PHY device's dev_flags
303  * @interface: PHY device's interface
304  */
305 int phy_connect_direct(struct net_device *dev, struct phy_device *phydev,
306 		       void (*handler)(struct net_device *), u32 flags,
307 		       phy_interface_t interface)
308 {
309 	int rc;
310 
311 	rc = phy_attach_direct(dev, phydev, flags, interface);
312 	if (rc)
313 		return rc;
314 
315 	phy_prepare_link(phydev, handler);
316 	phy_start_machine(phydev, NULL);
317 	if (phydev->irq > 0)
318 		phy_start_interrupts(phydev);
319 
320 	return 0;
321 }
322 EXPORT_SYMBOL(phy_connect_direct);
323 
324 /**
325  * phy_connect - connect an ethernet device to a PHY device
326  * @dev: the network device to connect
327  * @bus_id: the id string of the PHY device to connect
328  * @handler: callback function for state change notifications
329  * @flags: PHY device's dev_flags
330  * @interface: PHY device's interface
331  *
332  * Description: Convenience function for connecting ethernet
333  *   devices to PHY devices.  The default behavior is for
334  *   the PHY infrastructure to handle everything, and only notify
335  *   the connected driver when the link status changes.  If you
336  *   don't want, or can't use the provided functionality, you may
337  *   choose to call only the subset of functions which provide
338  *   the desired functionality.
339  */
340 struct phy_device * phy_connect(struct net_device *dev, const char *bus_id,
341 		void (*handler)(struct net_device *), u32 flags,
342 		phy_interface_t interface)
343 {
344 	struct phy_device *phydev;
345 	struct device *d;
346 	int rc;
347 
348 	/* Search the list of PHY devices on the mdio bus for the
349 	 * PHY with the requested name */
350 	d = bus_find_device_by_name(&mdio_bus_type, NULL, bus_id);
351 	if (!d) {
352 		pr_err("PHY %s not found\n", bus_id);
353 		return ERR_PTR(-ENODEV);
354 	}
355 	phydev = to_phy_device(d);
356 
357 	rc = phy_connect_direct(dev, phydev, handler, flags, interface);
358 	if (rc)
359 		return ERR_PTR(rc);
360 
361 	return phydev;
362 }
363 EXPORT_SYMBOL(phy_connect);
364 
365 /**
366  * phy_disconnect - disable interrupts, stop state machine, and detach a PHY device
367  * @phydev: target phy_device struct
368  */
369 void phy_disconnect(struct phy_device *phydev)
370 {
371 	if (phydev->irq > 0)
372 		phy_stop_interrupts(phydev);
373 
374 	phy_stop_machine(phydev);
375 
376 	phydev->adjust_link = NULL;
377 
378 	phy_detach(phydev);
379 }
380 EXPORT_SYMBOL(phy_disconnect);
381 
382 int phy_init_hw(struct phy_device *phydev)
383 {
384 	int ret;
385 
386 	if (!phydev->drv || !phydev->drv->config_init)
387 		return 0;
388 
389 	ret = phy_scan_fixups(phydev);
390 	if (ret < 0)
391 		return ret;
392 
393 	return phydev->drv->config_init(phydev);
394 }
395 
396 /**
397  * phy_attach_direct - attach a network device to a given PHY device pointer
398  * @dev: network device to attach
399  * @phydev: Pointer to phy_device to attach
400  * @flags: PHY device's dev_flags
401  * @interface: PHY device's interface
402  *
403  * Description: Called by drivers to attach to a particular PHY
404  *     device. The phy_device is found, and properly hooked up
405  *     to the phy_driver.  If no driver is attached, then the
406  *     genphy_driver is used.  The phy_device is given a ptr to
407  *     the attaching device, and given a callback for link status
408  *     change.  The phy_device is returned to the attaching driver.
409  */
410 int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
411 		      u32 flags, phy_interface_t interface)
412 {
413 	struct device *d = &phydev->dev;
414 
415 	/* Assume that if there is no driver, that it doesn't
416 	 * exist, and we should use the genphy driver. */
417 	if (NULL == d->driver) {
418 		int err;
419 		d->driver = &genphy_driver.driver;
420 
421 		err = d->driver->probe(d);
422 		if (err >= 0)
423 			err = device_bind_driver(d);
424 
425 		if (err)
426 			return err;
427 	}
428 
429 	if (phydev->attached_dev) {
430 		dev_err(&dev->dev, "PHY already attached\n");
431 		return -EBUSY;
432 	}
433 
434 	phydev->attached_dev = dev;
435 
436 	phydev->dev_flags = flags;
437 
438 	phydev->interface = interface;
439 
440 	/* Do initial configuration here, now that
441 	 * we have certain key parameters
442 	 * (dev_flags and interface) */
443 	return phy_init_hw(phydev);
444 }
445 EXPORT_SYMBOL(phy_attach_direct);
446 
447 /**
448  * phy_attach - attach a network device to a particular PHY device
449  * @dev: network device to attach
450  * @bus_id: Bus ID of PHY device to attach
451  * @flags: PHY device's dev_flags
452  * @interface: PHY device's interface
453  *
454  * Description: Same as phy_attach_direct() except that a PHY bus_id
455  *     string is passed instead of a pointer to a struct phy_device.
456  */
457 struct phy_device *phy_attach(struct net_device *dev,
458 		const char *bus_id, u32 flags, phy_interface_t interface)
459 {
460 	struct bus_type *bus = &mdio_bus_type;
461 	struct phy_device *phydev;
462 	struct device *d;
463 	int rc;
464 
465 	/* Search the list of PHY devices on the mdio bus for the
466 	 * PHY with the requested name */
467 	d = bus_find_device_by_name(bus, NULL, bus_id);
468 	if (!d) {
469 		pr_err("PHY %s not found\n", bus_id);
470 		return ERR_PTR(-ENODEV);
471 	}
472 	phydev = to_phy_device(d);
473 
474 	rc = phy_attach_direct(dev, phydev, flags, interface);
475 	if (rc)
476 		return ERR_PTR(rc);
477 
478 	return phydev;
479 }
480 EXPORT_SYMBOL(phy_attach);
481 
482 /**
483  * phy_detach - detach a PHY device from its network device
484  * @phydev: target phy_device struct
485  */
486 void phy_detach(struct phy_device *phydev)
487 {
488 	phydev->attached_dev = NULL;
489 
490 	/* If the device had no specific driver before (i.e. - it
491 	 * was using the generic driver), we unbind the device
492 	 * from the generic driver so that there's a chance a
493 	 * real driver could be loaded */
494 	if (phydev->dev.driver == &genphy_driver.driver)
495 		device_release_driver(&phydev->dev);
496 }
497 EXPORT_SYMBOL(phy_detach);
498 
499 
500 /* Generic PHY support and helper functions */
501 
502 /**
503  * genphy_config_advert - sanitize and advertise auto-negotation parameters
504  * @phydev: target phy_device struct
505  *
506  * Description: Writes MII_ADVERTISE with the appropriate values,
507  *   after sanitizing the values to make sure we only advertise
508  *   what is supported.  Returns < 0 on error, 0 if the PHY's advertisement
509  *   hasn't changed, and > 0 if it has changed.
510  */
511 int genphy_config_advert(struct phy_device *phydev)
512 {
513 	u32 advertise;
514 	int oldadv, adv;
515 	int err, changed = 0;
516 
517 	/* Only allow advertising what
518 	 * this PHY supports */
519 	phydev->advertising &= phydev->supported;
520 	advertise = phydev->advertising;
521 
522 	/* Setup standard advertisement */
523 	oldadv = adv = phy_read(phydev, MII_ADVERTISE);
524 
525 	if (adv < 0)
526 		return adv;
527 
528 	adv &= ~(ADVERTISE_ALL | ADVERTISE_100BASE4 | ADVERTISE_PAUSE_CAP |
529 		 ADVERTISE_PAUSE_ASYM);
530 	if (advertise & ADVERTISED_10baseT_Half)
531 		adv |= ADVERTISE_10HALF;
532 	if (advertise & ADVERTISED_10baseT_Full)
533 		adv |= ADVERTISE_10FULL;
534 	if (advertise & ADVERTISED_100baseT_Half)
535 		adv |= ADVERTISE_100HALF;
536 	if (advertise & ADVERTISED_100baseT_Full)
537 		adv |= ADVERTISE_100FULL;
538 	if (advertise & ADVERTISED_Pause)
539 		adv |= ADVERTISE_PAUSE_CAP;
540 	if (advertise & ADVERTISED_Asym_Pause)
541 		adv |= ADVERTISE_PAUSE_ASYM;
542 
543 	if (adv != oldadv) {
544 		err = phy_write(phydev, MII_ADVERTISE, adv);
545 
546 		if (err < 0)
547 			return err;
548 		changed = 1;
549 	}
550 
551 	/* Configure gigabit if it's supported */
552 	if (phydev->supported & (SUPPORTED_1000baseT_Half |
553 				SUPPORTED_1000baseT_Full)) {
554 		oldadv = adv = phy_read(phydev, MII_CTRL1000);
555 
556 		if (adv < 0)
557 			return adv;
558 
559 		adv &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
560 		if (advertise & SUPPORTED_1000baseT_Half)
561 			adv |= ADVERTISE_1000HALF;
562 		if (advertise & SUPPORTED_1000baseT_Full)
563 			adv |= ADVERTISE_1000FULL;
564 
565 		if (adv != oldadv) {
566 			err = phy_write(phydev, MII_CTRL1000, adv);
567 
568 			if (err < 0)
569 				return err;
570 			changed = 1;
571 		}
572 	}
573 
574 	return changed;
575 }
576 EXPORT_SYMBOL(genphy_config_advert);
577 
578 /**
579  * genphy_setup_forced - configures/forces speed/duplex from @phydev
580  * @phydev: target phy_device struct
581  *
582  * Description: Configures MII_BMCR to force speed/duplex
583  *   to the values in phydev. Assumes that the values are valid.
584  *   Please see phy_sanitize_settings().
585  */
586 int genphy_setup_forced(struct phy_device *phydev)
587 {
588 	int err;
589 	int ctl = 0;
590 
591 	phydev->pause = phydev->asym_pause = 0;
592 
593 	if (SPEED_1000 == phydev->speed)
594 		ctl |= BMCR_SPEED1000;
595 	else if (SPEED_100 == phydev->speed)
596 		ctl |= BMCR_SPEED100;
597 
598 	if (DUPLEX_FULL == phydev->duplex)
599 		ctl |= BMCR_FULLDPLX;
600 
601 	err = phy_write(phydev, MII_BMCR, ctl);
602 
603 	return err;
604 }
605 
606 
607 /**
608  * genphy_restart_aneg - Enable and Restart Autonegotiation
609  * @phydev: target phy_device struct
610  */
611 int genphy_restart_aneg(struct phy_device *phydev)
612 {
613 	int ctl;
614 
615 	ctl = phy_read(phydev, MII_BMCR);
616 
617 	if (ctl < 0)
618 		return ctl;
619 
620 	ctl |= (BMCR_ANENABLE | BMCR_ANRESTART);
621 
622 	/* Don't isolate the PHY if we're negotiating */
623 	ctl &= ~(BMCR_ISOLATE);
624 
625 	ctl = phy_write(phydev, MII_BMCR, ctl);
626 
627 	return ctl;
628 }
629 EXPORT_SYMBOL(genphy_restart_aneg);
630 
631 
632 /**
633  * genphy_config_aneg - restart auto-negotiation or write BMCR
634  * @phydev: target phy_device struct
635  *
636  * Description: If auto-negotiation is enabled, we configure the
637  *   advertising, and then restart auto-negotiation.  If it is not
638  *   enabled, then we write the BMCR.
639  */
640 int genphy_config_aneg(struct phy_device *phydev)
641 {
642 	int result;
643 
644 	if (AUTONEG_ENABLE != phydev->autoneg)
645 		return genphy_setup_forced(phydev);
646 
647 	result = genphy_config_advert(phydev);
648 
649 	if (result < 0) /* error */
650 		return result;
651 
652 	if (result == 0) {
653 		/* Advertisment hasn't changed, but maybe aneg was never on to
654 		 * begin with?  Or maybe phy was isolated? */
655 		int ctl = phy_read(phydev, MII_BMCR);
656 
657 		if (ctl < 0)
658 			return ctl;
659 
660 		if (!(ctl & BMCR_ANENABLE) || (ctl & BMCR_ISOLATE))
661 			result = 1; /* do restart aneg */
662 	}
663 
664 	/* Only restart aneg if we are advertising something different
665 	 * than we were before.	 */
666 	if (result > 0)
667 		result = genphy_restart_aneg(phydev);
668 
669 	return result;
670 }
671 EXPORT_SYMBOL(genphy_config_aneg);
672 
673 /**
674  * genphy_update_link - update link status in @phydev
675  * @phydev: target phy_device struct
676  *
677  * Description: Update the value in phydev->link to reflect the
678  *   current link value.  In order to do this, we need to read
679  *   the status register twice, keeping the second value.
680  */
681 int genphy_update_link(struct phy_device *phydev)
682 {
683 	int status;
684 
685 	/* Do a fake read */
686 	status = phy_read(phydev, MII_BMSR);
687 
688 	if (status < 0)
689 		return status;
690 
691 	/* Read link and autonegotiation status */
692 	status = phy_read(phydev, MII_BMSR);
693 
694 	if (status < 0)
695 		return status;
696 
697 	if ((status & BMSR_LSTATUS) == 0)
698 		phydev->link = 0;
699 	else
700 		phydev->link = 1;
701 
702 	return 0;
703 }
704 EXPORT_SYMBOL(genphy_update_link);
705 
706 /**
707  * genphy_read_status - check the link status and update current link state
708  * @phydev: target phy_device struct
709  *
710  * Description: Check the link, then figure out the current state
711  *   by comparing what we advertise with what the link partner
712  *   advertises.  Start by checking the gigabit possibilities,
713  *   then move on to 10/100.
714  */
715 int genphy_read_status(struct phy_device *phydev)
716 {
717 	int adv;
718 	int err;
719 	int lpa;
720 	int lpagb = 0;
721 
722 	/* Update the link, but return if there
723 	 * was an error */
724 	err = genphy_update_link(phydev);
725 	if (err)
726 		return err;
727 
728 	if (AUTONEG_ENABLE == phydev->autoneg) {
729 		if (phydev->supported & (SUPPORTED_1000baseT_Half
730 					| SUPPORTED_1000baseT_Full)) {
731 			lpagb = phy_read(phydev, MII_STAT1000);
732 
733 			if (lpagb < 0)
734 				return lpagb;
735 
736 			adv = phy_read(phydev, MII_CTRL1000);
737 
738 			if (adv < 0)
739 				return adv;
740 
741 			lpagb &= adv << 2;
742 		}
743 
744 		lpa = phy_read(phydev, MII_LPA);
745 
746 		if (lpa < 0)
747 			return lpa;
748 
749 		adv = phy_read(phydev, MII_ADVERTISE);
750 
751 		if (adv < 0)
752 			return adv;
753 
754 		lpa &= adv;
755 
756 		phydev->speed = SPEED_10;
757 		phydev->duplex = DUPLEX_HALF;
758 		phydev->pause = phydev->asym_pause = 0;
759 
760 		if (lpagb & (LPA_1000FULL | LPA_1000HALF)) {
761 			phydev->speed = SPEED_1000;
762 
763 			if (lpagb & LPA_1000FULL)
764 				phydev->duplex = DUPLEX_FULL;
765 		} else if (lpa & (LPA_100FULL | LPA_100HALF)) {
766 			phydev->speed = SPEED_100;
767 
768 			if (lpa & LPA_100FULL)
769 				phydev->duplex = DUPLEX_FULL;
770 		} else
771 			if (lpa & LPA_10FULL)
772 				phydev->duplex = DUPLEX_FULL;
773 
774 		if (phydev->duplex == DUPLEX_FULL){
775 			phydev->pause = lpa & LPA_PAUSE_CAP ? 1 : 0;
776 			phydev->asym_pause = lpa & LPA_PAUSE_ASYM ? 1 : 0;
777 		}
778 	} else {
779 		int bmcr = phy_read(phydev, MII_BMCR);
780 		if (bmcr < 0)
781 			return bmcr;
782 
783 		if (bmcr & BMCR_FULLDPLX)
784 			phydev->duplex = DUPLEX_FULL;
785 		else
786 			phydev->duplex = DUPLEX_HALF;
787 
788 		if (bmcr & BMCR_SPEED1000)
789 			phydev->speed = SPEED_1000;
790 		else if (bmcr & BMCR_SPEED100)
791 			phydev->speed = SPEED_100;
792 		else
793 			phydev->speed = SPEED_10;
794 
795 		phydev->pause = phydev->asym_pause = 0;
796 	}
797 
798 	return 0;
799 }
800 EXPORT_SYMBOL(genphy_read_status);
801 
802 static int genphy_config_init(struct phy_device *phydev)
803 {
804 	int val;
805 	u32 features;
806 
807 	/* For now, I'll claim that the generic driver supports
808 	 * all possible port types */
809 	features = (SUPPORTED_TP | SUPPORTED_MII
810 			| SUPPORTED_AUI | SUPPORTED_FIBRE |
811 			SUPPORTED_BNC);
812 
813 	/* Do we support autonegotiation? */
814 	val = phy_read(phydev, MII_BMSR);
815 
816 	if (val < 0)
817 		return val;
818 
819 	if (val & BMSR_ANEGCAPABLE)
820 		features |= SUPPORTED_Autoneg;
821 
822 	if (val & BMSR_100FULL)
823 		features |= SUPPORTED_100baseT_Full;
824 	if (val & BMSR_100HALF)
825 		features |= SUPPORTED_100baseT_Half;
826 	if (val & BMSR_10FULL)
827 		features |= SUPPORTED_10baseT_Full;
828 	if (val & BMSR_10HALF)
829 		features |= SUPPORTED_10baseT_Half;
830 
831 	if (val & BMSR_ESTATEN) {
832 		val = phy_read(phydev, MII_ESTATUS);
833 
834 		if (val < 0)
835 			return val;
836 
837 		if (val & ESTATUS_1000_TFULL)
838 			features |= SUPPORTED_1000baseT_Full;
839 		if (val & ESTATUS_1000_THALF)
840 			features |= SUPPORTED_1000baseT_Half;
841 	}
842 
843 	phydev->supported = features;
844 	phydev->advertising = features;
845 
846 	return 0;
847 }
848 int genphy_suspend(struct phy_device *phydev)
849 {
850 	int value;
851 
852 	mutex_lock(&phydev->lock);
853 
854 	value = phy_read(phydev, MII_BMCR);
855 	phy_write(phydev, MII_BMCR, (value | BMCR_PDOWN));
856 
857 	mutex_unlock(&phydev->lock);
858 
859 	return 0;
860 }
861 EXPORT_SYMBOL(genphy_suspend);
862 
863 int genphy_resume(struct phy_device *phydev)
864 {
865 	int value;
866 
867 	mutex_lock(&phydev->lock);
868 
869 	value = phy_read(phydev, MII_BMCR);
870 	phy_write(phydev, MII_BMCR, (value & ~BMCR_PDOWN));
871 
872 	mutex_unlock(&phydev->lock);
873 
874 	return 0;
875 }
876 EXPORT_SYMBOL(genphy_resume);
877 
878 /**
879  * phy_probe - probe and init a PHY device
880  * @dev: device to probe and init
881  *
882  * Description: Take care of setting up the phy_device structure,
883  *   set the state to READY (the driver's init function should
884  *   set it to STARTING if needed).
885  */
886 static int phy_probe(struct device *dev)
887 {
888 	struct phy_device *phydev;
889 	struct phy_driver *phydrv;
890 	struct device_driver *drv;
891 	int err = 0;
892 
893 	phydev = to_phy_device(dev);
894 
895 	/* Make sure the driver is held.
896 	 * XXX -- Is this correct? */
897 	drv = get_driver(phydev->dev.driver);
898 	phydrv = to_phy_driver(drv);
899 	phydev->drv = phydrv;
900 
901 	/* Disable the interrupt if the PHY doesn't support it */
902 	if (!(phydrv->flags & PHY_HAS_INTERRUPT))
903 		phydev->irq = PHY_POLL;
904 
905 	mutex_lock(&phydev->lock);
906 
907 	/* Start out supporting everything. Eventually,
908 	 * a controller will attach, and may modify one
909 	 * or both of these values */
910 	phydev->supported = phydrv->features;
911 	phydev->advertising = phydrv->features;
912 
913 	/* Set the state to READY by default */
914 	phydev->state = PHY_READY;
915 
916 	if (phydev->drv->probe)
917 		err = phydev->drv->probe(phydev);
918 
919 	mutex_unlock(&phydev->lock);
920 
921 	return err;
922 
923 }
924 
925 static int phy_remove(struct device *dev)
926 {
927 	struct phy_device *phydev;
928 
929 	phydev = to_phy_device(dev);
930 
931 	mutex_lock(&phydev->lock);
932 	phydev->state = PHY_DOWN;
933 	mutex_unlock(&phydev->lock);
934 
935 	if (phydev->drv->remove)
936 		phydev->drv->remove(phydev);
937 
938 	put_driver(dev->driver);
939 	phydev->drv = NULL;
940 
941 	return 0;
942 }
943 
944 /**
945  * phy_driver_register - register a phy_driver with the PHY layer
946  * @new_driver: new phy_driver to register
947  */
948 int phy_driver_register(struct phy_driver *new_driver)
949 {
950 	int retval;
951 
952 	new_driver->driver.name = new_driver->name;
953 	new_driver->driver.bus = &mdio_bus_type;
954 	new_driver->driver.probe = phy_probe;
955 	new_driver->driver.remove = phy_remove;
956 
957 	retval = driver_register(&new_driver->driver);
958 
959 	if (retval) {
960 		printk(KERN_ERR "%s: Error %d in registering driver\n",
961 				new_driver->name, retval);
962 
963 		return retval;
964 	}
965 
966 	pr_debug("%s: Registered new driver\n", new_driver->name);
967 
968 	return 0;
969 }
970 EXPORT_SYMBOL(phy_driver_register);
971 
972 void phy_driver_unregister(struct phy_driver *drv)
973 {
974 	driver_unregister(&drv->driver);
975 }
976 EXPORT_SYMBOL(phy_driver_unregister);
977 
978 static struct phy_driver genphy_driver = {
979 	.phy_id		= 0xffffffff,
980 	.phy_id_mask	= 0xffffffff,
981 	.name		= "Generic PHY",
982 	.config_init	= genphy_config_init,
983 	.features	= 0,
984 	.config_aneg	= genphy_config_aneg,
985 	.read_status	= genphy_read_status,
986 	.suspend	= genphy_suspend,
987 	.resume		= genphy_resume,
988 	.driver		= {.owner= THIS_MODULE, },
989 };
990 
991 static int __init phy_init(void)
992 {
993 	int rc;
994 
995 	rc = mdio_bus_init();
996 	if (rc)
997 		return rc;
998 
999 	rc = phy_driver_register(&genphy_driver);
1000 	if (rc)
1001 		mdio_bus_exit();
1002 
1003 	return rc;
1004 }
1005 
1006 static void __exit phy_exit(void)
1007 {
1008 	phy_driver_unregister(&genphy_driver);
1009 	mdio_bus_exit();
1010 }
1011 
1012 subsys_initcall(phy_init);
1013 module_exit(phy_exit);
1014