static int phylink_phy_write(struct phylink *pl, unsigned int phy_id,
unsigned int reg, unsigned int val)
{
struct phy_device *phydev = pl->phydev;
int prtad, devad;
if (mdio_phy_id_is_c45(phy_id)) {
prtad = mdio_phy_id_prtad(phy_id);
devad = mdio_phy_id_devad(phy_id);
devad = MII_ADDR_C45 | devad << 16 | reg;
} else if (phydev->is_c45) {
switch (reg) {
case MII_BMCR:
case MII_BMSR:
case MII_PHYSID1:
case MII_PHYSID2:
devad = __ffs(phydev->c45_ids.devices_in_package);
break;
case MII_ADVERTISE:
case MII_LPA:
if (!(phydev->c45_ids.devices_in_package & MDIO_DEVS_AN))
return -EINVAL;
devad = MDIO_MMD_AN;
if (reg == MII_ADVERTISE)
reg = MDIO_AN_ADVERTISE;
else
reg = MDIO_AN_LPA;
break;
default:
return -EINVAL;
}
prtad = phy_id;
devad = MII_ADDR_C45 | devad << 16 | reg;
} else {
prtad = phy_id;
devad = reg;
}
return mdiobus_write(phydev->mdio.bus, prtad, devad, val);
}
/**
* mdio_mii_ioctl - MII ioctl interface for MDIO (clause 22 or 45) PHYs
* @mdio: MDIO interface
* @mii_data: MII ioctl data structure
* @cmd: MII ioctl command
*
* Returns 0 on success, negative on error.
*/
int mdio_mii_ioctl(const struct mdio_if_info *mdio,
struct mii_ioctl_data *mii_data, int cmd)
{
int prtad, devad;
u16 addr = mii_data->reg_num;
/* Validate/convert cmd to one of SIOC{G,S}MIIREG */
switch (cmd) {
case SIOCGMIIPHY:
if (mdio->prtad == MDIO_PRTAD_NONE)
return -EOPNOTSUPP;
mii_data->phy_id = mdio->prtad;
cmd = SIOCGMIIREG;
break;
case SIOCGMIIREG:
case SIOCSMIIREG:
break;
default:
return -EOPNOTSUPP;
}
/* Validate/convert phy_id */
if ((mdio->mode_support & MDIO_SUPPORTS_C45) &&
mdio_phy_id_is_c45(mii_data->phy_id)) {
prtad = mdio_phy_id_prtad(mii_data->phy_id);
devad = mdio_phy_id_devad(mii_data->phy_id);
} else if ((mdio->mode_support & MDIO_SUPPORTS_C22) &&
mii_data->phy_id < 0x20) {
prtad = mii_data->phy_id;
devad = MDIO_DEVAD_NONE;
addr &= 0x1f;
} else if ((mdio->mode_support & MDIO_EMULATE_C22) &&
mdio->prtad != MDIO_PRTAD_NONE &&
mii_data->phy_id == mdio->prtad) {
/* Remap commonly-used MII registers. */
prtad = mdio->prtad;
switch (addr) {
case MII_BMCR:
case MII_BMSR:
case MII_PHYSID1:
case MII_PHYSID2:
devad = __ffs(mdio->mmds);
break;
case MII_ADVERTISE:
case MII_LPA:
if (!(mdio->mmds & MDIO_DEVS_AN))
return -EINVAL;
devad = MDIO_MMD_AN;
if (addr == MII_ADVERTISE)
addr = MDIO_AN_ADVERTISE;
else
addr = MDIO_AN_LPA;
break;
default:
return -EINVAL;
}
} else {
return -EINVAL;
}
if (cmd == SIOCGMIIREG) {
int rc = mdio->mdio_read(mdio->dev, prtad, devad, addr);
if (rc < 0)
return rc;
mii_data->val_out = rc;
return 0;
} else {
return mdio->mdio_write(mdio->dev, prtad, devad, addr,
mii_data->val_in);
}
}
