diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c index 9702d042296..b58283fe3d5 100644 --- a/drivers/net/phy/phy.c +++ b/drivers/net/phy/phy.c @@ -1250,3 +1250,116 @@ bool phy_interface_is_ncsi(void) return 0; #endif } + +/** + * __phy_read_page() - read the current page + * @phydev: a pointer to a &struct phy_device + * + * Returns page index or < 0 on error + */ +static int __phy_read_page(struct phy_device *phydev) +{ + struct phy_driver *drv = phydev->drv; + + if (!drv->read_page) { + debug("read_page callback not available, PHY driver not loaded?\n"); + return -EOPNOTSUPP; + } + + return drv->read_page(phydev); +} + +/** + * __phy_write_page() - Write a new page + * @phydev: a pointer to a &struct phy_device + * @page: page index to select + * + * Returns 0 or < 0 on error. + */ +static int __phy_write_page(struct phy_device *phydev, int page) +{ + struct phy_driver *drv = phydev->drv; + + if (!drv->write_page) { + debug("write_page callback not available, PHY driver not loaded?\n"); + return -EOPNOTSUPP; + } + + return drv->write_page(phydev, page); +} + +/** + * phy_save_page() - save the current page + * @phydev: a pointer to a &struct phy_device + * + * Return the current page number. On error, + * returns a negative errno. phy_restore_page() must always be called + * after this, irrespective of success or failure of this call. + */ +int phy_save_page(struct phy_device *phydev) +{ + return __phy_read_page(phydev); +} + +/** + * phy_select_page - Switch to a PHY page and return the previous page + * @phydev: a pointer to a &struct phy_device + * @page: desired page + * + * NOTE: Save the current PHY page, and set the current page. + * On error, returns a negative errno, otherwise returns the previous page number. + * phy_restore_page() must always be called after this, irrespective + * of success or failure of this call. + */ +int phy_select_page(struct phy_device *phydev, int page) +{ + int ret, oldpage; + + oldpage = ret = phy_save_page(phydev); + if (ret < 0) + return ret; + + if (oldpage != page) { + ret = __phy_write_page(phydev, page); + if (ret < 0) + return ret; + } + + return oldpage; +} + +/** + * phy_restore_page - Restore a previously saved page and propagate status + * @phydev: a pointer to a &struct phy_device + * @oldpage: the old page, return value from phy_save_page() or phy_select_page() + * @ret: operation's return code + * + * Restoring @oldpage if it is a valid page. + * This function propagates the earliest error code from the group of + * operations. + * + * Returns: + * @oldpage if it was a negative value, otherwise + * @ret if it was a negative errno value, otherwise + * phy_write_page()'s negative value if it were in error, otherwise + * @ret. + */ +int phy_restore_page(struct phy_device *phydev, int oldpage, int ret) +{ + int r; + + if (oldpage >= 0) { + r = __phy_write_page(phydev, oldpage); + + /* Propagate the operation return code if the page write + * was successful. + */ + if (ret >= 0 && r < 0) + ret = r; + } else { + /* Propagate the phy page selection error code */ + ret = oldpage; + } + + return ret; +} \ No newline at end of file diff --git a/include/phy.h b/include/phy.h index 36354aaf774..ae9fd1652cc 100644 --- a/include/phy.h +++ b/include/phy.h @@ -123,6 +123,11 @@ struct phy_driver { int (*write_mmd)(struct phy_device *phydev, int devad, int reg, u16 val); + /** @read_page: Return the current PHY register page number */ + int (*read_page)(struct phy_device *phydev); + /** @write_page: Set the current PHY register page number */ + int (*write_page)(struct phy_device *phydev, int page); + /* driver private data */ ulong data; }; @@ -314,6 +319,9 @@ int phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum, u16 mask, u16 set); int phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 mask, u16 set); +int phy_save_page(struct phy_device *phydev); +int phy_select_page(struct phy_device *phydev, int page); +int phy_restore_page(struct phy_device *phydev, int oldpage, int ret); int phy_startup(struct phy_device *phydev); int phy_config(struct phy_device *phydev);