target/linux/realtek/patches-5.10/712-net-phy-sfp-add-support-for-SMBus.patch

   1 From 3cb0bde365d913c484d20224367a54a0eac780a7 Mon Sep 17 00:00:00 2001
   2 From: Antoine Tenart <antoine.tenart@bootlin.com>
   3 Date: Fri, 21 Feb 2020 11:55:29 +0100
   4 Subject: [PATCH 3/3] net: phy: sfp: add support for SMBus
   5
   6 Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
   7 ---
   8  drivers/net/phy/sfp.c | 68 ++++++++++++++++++++++++++++++++++---------
   9  1 file changed, 54 insertions(+), 14 deletions(-)
  10
  11 --- a/drivers/net/phy/sfp.c
  12 +++ b/drivers/net/phy/sfp.c
  13 @@ -419,32 +419,72 @@ static int sfp_i2c_write(struct sfp *sfp
  14         return ret == ARRAY_SIZE(msgs) ? len : 0;
  15  }
  16
  17 +static int sfp_smbus_read(struct sfp *sfp, bool a2, u8 dev_addr, void *buf,
  18 +                         size_t len)
  19 +{
  20 +       u8 bus_addr = a2 ? 0x51 : 0x50, *val = buf;
  21 +
  22 +       bus_addr -= 0x40;
  23 +
  24 +       while (len > 0) {
  25 +               *val = sfp->i2c_mii->read(sfp->i2c_mii, bus_addr, dev_addr);
  26 +
  27 +               val++;
  28 +               dev_addr++;
  29 +               len--;
  30 +       }
  31 +
  32 +       return val - (u8 *)buf;
  33 +}
  34 +
  35 +static int sfp_smbus_write(struct sfp *sfp, bool a2, u8 dev_addr, void *buf,
  36 +                          size_t len)
  37 +{
  38 +       u8 bus_addr = a2 ? 0x51 : 0x50;
  39 +       u16 val;
  40 +
  41 +       memcpy(&val, buf, len);
  42 +
  43 +       return sfp->i2c_mii->write(sfp->i2c_mii, bus_addr, dev_addr, val);
  44 +}
  45 +
  46  static int sfp_i2c_configure(struct sfp *sfp, struct i2c_adapter *i2c)
  47  {
  48 -       struct mii_bus *i2c_mii;
  49 +       struct mii_bus *mii;
  50         int ret;
  51
  52 -       if (!i2c_check_functionality(i2c, I2C_FUNC_I2C))
  53 -               return -EINVAL;
  54 -
  55         sfp->i2c = i2c;
  56 -       sfp->read = sfp_i2c_read;
  57 -       sfp->write = sfp_i2c_write;
  58
  59 -       i2c_mii = mdio_i2c_alloc(sfp->dev, i2c);
  60 -       if (IS_ERR(i2c_mii))
  61 -               return PTR_ERR(i2c_mii);
  62 +       if (i2c_check_functionality(i2c, I2C_FUNC_I2C)) {
  63 +               sfp->read = sfp_i2c_read;
  64 +               sfp->write = sfp_i2c_write;
  65 +
  66 +               mii = mdio_i2c_alloc(sfp->dev, i2c);
  67 +               if (IS_ERR(mii))
  68 +                       return PTR_ERR(mii);
  69 +
  70 +               mii->name = "SFP I2C Bus";
  71 +       } else if (i2c_check_functionality(i2c, I2C_FUNC_SMBUS_BYTE_DATA)) {
  72 +               sfp->read = sfp_smbus_read;
  73 +               sfp->write = sfp_smbus_write;
  74 +
  75 +               mii = mdio_smbus_alloc(sfp->dev, i2c);
  76 +               if (IS_ERR(mii))
  77 +                       return PTR_ERR(mii);
  78
  79 -       i2c_mii->name = "SFP I2C Bus";
  80 -       i2c_mii->phy_mask = ~0;
  81 +               mii->name = "SFP SMBus";
  82 +       } else {
  83 +               return -EINVAL;
  84 +       }
  85
  86 -       ret = mdiobus_register(i2c_mii);
  87 +       mii->phy_mask = ~0;
  88 +       ret = mdiobus_register(mii);
  89         if (ret < 0) {
  90 -               mdiobus_free(i2c_mii);
  91 +               mdiobus_free(mii);
  92                 return ret;
  93         }
  94
  95 -       sfp->i2c_mii = i2c_mii;
  96 +       sfp->i2c_mii = mii;
  97
  98         return 0;
  99  }