[][kernel][common][eth][Fix 10GBASE-T SFP+ IOT issue]
[Description]
Fix 10GBASE-T SFP+ IOT issue.
- Backport MDIO I2C ROLLBALL mode
- Support OEM TNBYV02-C0X-C3 SFP+ module
- Support JESS-LINK P60000BBC001-1 module
- Fix panic issue for Marvell 10G PHY interface down/up
If without this patch, SFP framework might fail to attach Ethernet
PHY driver.
[Release-log]
N/A
Change-Id: I3b5cc8a4cc1da988b5cc3dd02a6ae688792403c2
Reviewed-on: https://gerrit.mediatek.inc/c/openwrt/feeds/mtk_openwrt_feeds/+/6966161
diff --git a/target/linux/mediatek/patches-5.4/754-net-phy-sfp-add-rollball-support.patch b/target/linux/mediatek/patches-5.4/754-net-phy-sfp-add-rollball-support.patch
new file mode 100644
index 0000000..ef1f3cb
--- /dev/null
+++ b/target/linux/mediatek/patches-5.4/754-net-phy-sfp-add-rollball-support.patch
@@ -0,0 +1,1024 @@
+diff --git a/drivers/net/phy/mdio-i2c.c b/drivers/net/phy/mdio-i2c.c
+index 0746e2c..81c8fe7 100644
+--- a/drivers/net/phy/mdio-i2c.c
++++ b/drivers/net/phy/mdio-i2c.c
+@@ -11,6 +11,7 @@
+ */
+ #include <linux/i2c.h>
+ #include <linux/phy.h>
++#include <linux/sfp.h>
+
+ #include "mdio-i2c.h"
+
+@@ -29,7 +30,7 @@ static unsigned int i2c_mii_phy_addr(int phy_id)
+ return phy_id + 0x40;
+ }
+
+-static int i2c_mii_read(struct mii_bus *bus, int phy_id, int reg)
++static int i2c_mii_read_default(struct mii_bus *bus, int phy_id, int reg)
+ {
+ struct i2c_adapter *i2c = bus->priv;
+ struct i2c_msg msgs[2];
+@@ -63,7 +64,8 @@ static int i2c_mii_read(struct mii_bus *bus, int phy_id, int reg)
+ return data[0] << 8 | data[1];
+ }
+
+-static int i2c_mii_write(struct mii_bus *bus, int phy_id, int reg, u16 val)
++static int i2c_mii_write_default(struct mii_bus *bus, int phy_id, int reg,
++ u16 val)
+ {
+ struct i2c_adapter *i2c = bus->priv;
+ struct i2c_msg msg;
+@@ -92,9 +94,288 @@ static int i2c_mii_write(struct mii_bus *bus, int phy_id, int reg, u16 val)
+ return ret < 0 ? ret : 0;
+ }
+
+-struct mii_bus *mdio_i2c_alloc(struct device *parent, struct i2c_adapter *i2c)
++/* RollBall SFPs do not access internal PHY via I2C address 0x56, but
++ * instead via address 0x51, when SFP page is set to 0x03 and password to
++ * 0xffffffff.
++ *
++ * address size contents description
++ * ------- ---- -------- -----------
++ * 0x80 1 CMD 0x01/0x02/0x04 for write/read/done
++ * 0x81 1 DEV Clause 45 device
++ * 0x82 2 REG Clause 45 register
++ * 0x84 2 VAL Register value
++ */
++#define ROLLBALL_PHY_I2C_ADDR 0x51
++
++#define ROLLBALL_PASSWORD (SFP_VSL + 3)
++
++#define ROLLBALL_CMD_ADDR 0x80
++#define ROLLBALL_DATA_ADDR 0x81
++
++#define ROLLBALL_CMD_WRITE 0x01
++#define ROLLBALL_CMD_READ 0x02
++#define ROLLBALL_CMD_DONE 0x04
++
++#define SFP_PAGE_ROLLBALL_MDIO 3
++
++static int __i2c_transfer_err(struct i2c_adapter *i2c, struct i2c_msg *msgs,
++ int num)
++{
++ int ret;
++
++ ret = __i2c_transfer(i2c, msgs, num);
++ if (ret < 0)
++ return ret;
++ else if (ret != num)
++ return -EIO;
++ else
++ return 0;
++}
++
++static int __i2c_rollball_get_page(struct i2c_adapter *i2c, int bus_addr,
++ u8 *page)
++{
++ struct i2c_msg msgs[2];
++ u8 addr = SFP_PAGE;
++
++ msgs[0].addr = bus_addr;
++ msgs[0].flags = 0;
++ msgs[0].len = 1;
++ msgs[0].buf = &addr;
++
++ msgs[1].addr = bus_addr;
++ msgs[1].flags = I2C_M_RD;
++ msgs[1].len = 1;
++ msgs[1].buf = page;
++
++ return __i2c_transfer_err(i2c, msgs, 2);
++}
++
++static int __i2c_rollball_set_page(struct i2c_adapter *i2c, int bus_addr,
++ u8 page)
++{
++ struct i2c_msg msg;
++ u8 buf[2];
++
++ buf[0] = SFP_PAGE;
++ buf[1] = page;
++
++ msg.addr = bus_addr;
++ msg.flags = 0;
++ msg.len = 2;
++ msg.buf = buf;
++
++ return __i2c_transfer_err(i2c, &msg, 1);
++}
++
++/* In order to not interfere with other SFP code (which possibly may manipulate
++ * SFP_PAGE), for every transfer we do this:
++ * 1. lock the bus
++ * 2. save content of SFP_PAGE
++ * 3. set SFP_PAGE to 3
++ * 4. do the transfer
++ * 5. restore original SFP_PAGE
++ * 6. unlock the bus
++ * Note that one might think that steps 2 to 5 could be theoretically done all
++ * in one call to i2c_transfer (by constructing msgs array in such a way), but
++ * unfortunately tests show that this does not work :-( Changed SFP_PAGE does
++ * not take into account until i2c_transfer() is done.
++ */
++static int i2c_transfer_rollball(struct i2c_adapter *i2c,
++ struct i2c_msg *msgs, int num)
++{
++ int ret, main_err = 0;
++ u8 saved_page;
++
++ i2c_lock_bus(i2c, I2C_LOCK_SEGMENT);
++
++ /* save original page */
++ ret = __i2c_rollball_get_page(i2c, msgs->addr, &saved_page);
++ if (ret)
++ goto unlock;
++
++ /* change to RollBall MDIO page */
++ ret = __i2c_rollball_set_page(i2c, msgs->addr, SFP_PAGE_ROLLBALL_MDIO);
++ if (ret)
++ goto unlock;
++
++ /* do the transfer; we try to restore original page if this fails */
++ ret = __i2c_transfer_err(i2c, msgs, num);
++ if (ret)
++ main_err = ret;
++
++ /* restore original page */
++ ret = __i2c_rollball_set_page(i2c, msgs->addr, saved_page);
++
++unlock:
++ i2c_unlock_bus(i2c, I2C_LOCK_SEGMENT);
++
++ return main_err ? : ret;
++}
++
++static int i2c_rollball_mii_poll(struct mii_bus *bus, int bus_addr, u8 *buf,
++ size_t len)
++{
++ struct i2c_adapter *i2c = bus->priv;
++ struct i2c_msg msgs[2];
++ u8 cmd_addr, tmp, *res;
++ int i, ret;
++
++ cmd_addr = ROLLBALL_CMD_ADDR;
++
++ res = buf ? buf : &tmp;
++ len = buf ? len : 1;
++
++ msgs[0].addr = bus_addr;
++ msgs[0].flags = 0;
++ msgs[0].len = 1;
++ msgs[0].buf = &cmd_addr;
++
++ msgs[1].addr = bus_addr;
++ msgs[1].flags = I2C_M_RD;
++ msgs[1].len = len;
++ msgs[1].buf = res;
++
++ /* By experiment it takes up to 70 ms to access a register for these
++ * SFPs. Sleep 20ms between iterations and try 10 times.
++ */
++ i = 10;
++ do {
++ msleep(20);
++
++ ret = i2c_transfer_rollball(i2c, msgs, ARRAY_SIZE(msgs));
++ if (ret)
++ return ret;
++
++ if (*res == ROLLBALL_CMD_DONE)
++ return 0;
++ } while (i-- > 0);
++
++ dev_info(&bus->dev, "poll timed out\n");
++
++ return -ETIMEDOUT;
++}
++
++static int i2c_rollball_mii_cmd(struct mii_bus *bus, int bus_addr, u8 cmd,
++ u8 *data, size_t len)
++{
++ struct i2c_adapter *i2c = bus->priv;
++ struct i2c_msg msgs[2];
++ u8 cmdbuf[2];
++
++ cmdbuf[0] = ROLLBALL_CMD_ADDR;
++ cmdbuf[1] = cmd;
++
++ msgs[0].addr = bus_addr;
++ msgs[0].flags = 0;
++ msgs[0].len = len;
++ msgs[0].buf = data;
++
++ msgs[1].addr = bus_addr;
++ msgs[1].flags = 0;
++ msgs[1].len = sizeof(cmdbuf);
++ msgs[1].buf = cmdbuf;
++
++ return i2c_transfer_rollball(i2c, msgs, ARRAY_SIZE(msgs));
++}
++
++static int i2c_mii_read_rollball(struct mii_bus *bus, int phy_id, int reg)
++{
++ u8 buf[4], res[6];
++ int bus_addr, ret;
++ u16 val;
++
++ if (!(reg & MII_ADDR_C45))
++ return -EOPNOTSUPP;
++
++ bus_addr = i2c_mii_phy_addr(phy_id);
++ if (bus_addr != ROLLBALL_PHY_I2C_ADDR)
++ return 0xffff;
++
++ buf[0] = ROLLBALL_DATA_ADDR;
++ buf[1] = (reg >> 16) & 0x1f;
++ buf[2] = (reg >> 8) & 0xff;
++ buf[3] = reg & 0xff;
++
++ ret = i2c_rollball_mii_cmd(bus, bus_addr, ROLLBALL_CMD_READ, buf,
++ sizeof(buf));
++ if (ret < 0)
++ return ret;
++
++ ret = i2c_rollball_mii_poll(bus, bus_addr, res, sizeof(res));
++ if (ret == -ETIMEDOUT)
++ return 0xffff;
++ else if (ret < 0)
++ return ret;
++
++ val = res[4] << 8 | res[5];
++
++ return val;
++}
++
++static int i2c_mii_write_rollball(struct mii_bus *bus, int phy_id, int reg,
++ u16 val)
++{
++ int bus_addr, ret;
++ u8 buf[6];
++
++ if (!(reg & MII_ADDR_C45))
++ return -EOPNOTSUPP;
++
++ bus_addr = i2c_mii_phy_addr(phy_id);
++ if (bus_addr != ROLLBALL_PHY_I2C_ADDR)
++ return 0;
++
++ buf[0] = ROLLBALL_DATA_ADDR;
++ buf[1] = (reg >> 16) & 0x1f;
++ buf[2] = (reg >> 8) & 0xff;
++ buf[3] = reg & 0xff;
++ buf[4] = val >> 8;
++ buf[5] = val & 0xff;
++
++ ret = i2c_rollball_mii_cmd(bus, bus_addr, ROLLBALL_CMD_WRITE, buf,
++ sizeof(buf));
++ if (ret < 0)
++ return ret;
++
++ ret = i2c_rollball_mii_poll(bus, bus_addr, NULL, 0);
++ if (ret < 0)
++ return ret;
++
++ return 0;
++}
++
++static int i2c_mii_init_rollball(struct i2c_adapter *i2c)
++{
++ struct i2c_msg msg;
++ u8 pw[5];
++ int ret;
++
++ pw[0] = ROLLBALL_PASSWORD;
++ pw[1] = 0xff;
++ pw[2] = 0xff;
++ pw[3] = 0xff;
++ pw[4] = 0xff;
++
++ msg.addr = ROLLBALL_PHY_I2C_ADDR;
++ msg.flags = 0;
++ msg.len = sizeof(pw);
++ msg.buf = pw;
++
++ ret = i2c_transfer(i2c, &msg, 1);
++ if (ret < 0)
++ return ret;
++ else if (ret != 1)
++ return -EIO;
++ else
++ return 0;
++}
++
++struct mii_bus *mdio_i2c_alloc(struct device *parent, struct i2c_adapter *i2c,
++ enum mdio_i2c_proto protocol)
+ {
+ struct mii_bus *mii;
++ int ret;
+
+ if (!i2c_check_functionality(i2c, I2C_FUNC_I2C))
+ return ERR_PTR(-EINVAL);
+@@ -105,10 +386,28 @@ struct mii_bus *mdio_i2c_alloc(struct device *parent, struct i2c_adapter *i2c)
+
+ snprintf(mii->id, MII_BUS_ID_SIZE, "i2c:%s", dev_name(parent));
+ mii->parent = parent;
+- mii->read = i2c_mii_read;
+- mii->write = i2c_mii_write;
+ mii->priv = i2c;
+
++ switch (protocol) {
++ case MDIO_I2C_ROLLBALL:
++ ret = i2c_mii_init_rollball(i2c);
++ if (ret < 0) {
++ dev_err(parent,
++ "Cannot initialize RollBall MDIO I2C protocol: %d\n",
++ ret);
++ mdiobus_free(mii);
++ return ERR_PTR(ret);
++ }
++
++ mii->read = i2c_mii_read_rollball;
++ mii->write = i2c_mii_write_rollball;
++ break;
++ default:
++ mii->read = i2c_mii_read_default;
++ mii->write = i2c_mii_write_default;
++ break;
++ }
++
+ return mii;
+ }
+ EXPORT_SYMBOL_GPL(mdio_i2c_alloc);
+diff --git a/drivers/net/phy/mdio-i2c.h b/drivers/net/phy/mdio-i2c.h
+index 751dab2..1c21140 100644
+--- a/drivers/net/phy/mdio-i2c.h
++++ b/drivers/net/phy/mdio-i2c.h
+@@ -11,6 +11,14 @@ struct device;
+ struct i2c_adapter;
+ struct mii_bus;
+
+-struct mii_bus *mdio_i2c_alloc(struct device *parent, struct i2c_adapter *i2c);
++enum mdio_i2c_proto {
++ MDIO_I2C_NONE,
++ MDIO_I2C_MARVELL_C22,
++ MDIO_I2C_C45,
++ MDIO_I2C_ROLLBALL,
++};
++
++struct mii_bus *mdio_i2c_alloc(struct device *parent, struct i2c_adapter *i2c,
++ enum mdio_i2c_proto protocol);
+
+ #endif
+diff --git a/drivers/net/phy/sfp-bus.c b/drivers/net/phy/sfp-bus.c
+index 42f0441..0d5ac2a 100644
+--- a/drivers/net/phy/sfp-bus.c
++++ b/drivers/net/phy/sfp-bus.c
+@@ -10,12 +10,6 @@
+
+ #include "sfp.h"
+
+-struct sfp_quirk {
+- const char *vendor;
+- const char *part;
+- void (*modes)(const struct sfp_eeprom_id *id, unsigned long *modes);
+-};
+-
+ /**
+ * struct sfp_bus - internal representation of a sfp bus
+ */
+@@ -38,87 +32,6 @@ struct sfp_bus {
+ bool started;
+ };
+
+-static void sfp_quirk_2500basex(const struct sfp_eeprom_id *id,
+- unsigned long *modes)
+-{
+- phylink_set(modes, 2500baseX_Full);
+-}
+-
+-static void sfp_quirk_ubnt_uf_instant(const struct sfp_eeprom_id *id,
+- unsigned long *modes)
+-{
+- /* Ubiquiti U-Fiber Instant module claims that support all transceiver
+- * types including 10G Ethernet which is not truth. So clear all claimed
+- * modes and set only one mode which module supports: 1000baseX_Full.
+- */
+- phylink_zero(modes);
+- phylink_set(modes, 1000baseX_Full);
+-}
+-
+-static const struct sfp_quirk sfp_quirks[] = {
+- {
+- // Alcatel Lucent G-010S-P can operate at 2500base-X, but
+- // incorrectly report 2500MBd NRZ in their EEPROM
+- .vendor = "ALCATELLUCENT",
+- .part = "G010SP",
+- .modes = sfp_quirk_2500basex,
+- }, {
+- // Alcatel Lucent G-010S-A can operate at 2500base-X, but
+- // report 3.2GBd NRZ in their EEPROM
+- .vendor = "ALCATELLUCENT",
+- .part = "3FE46541AA",
+- .modes = sfp_quirk_2500basex,
+- }, {
+- // Huawei MA5671A can operate at 2500base-X, but report 1.2GBd
+- // NRZ in their EEPROM
+- .vendor = "HUAWEI",
+- .part = "MA5671A",
+- .modes = sfp_quirk_2500basex,
+- }, {
+- .vendor = "UBNT",
+- .part = "UF-INSTANT",
+- .modes = sfp_quirk_ubnt_uf_instant,
+- },
+-};
+-
+-static size_t sfp_strlen(const char *str, size_t maxlen)
+-{
+- size_t size, i;
+-
+- /* Trailing characters should be filled with space chars */
+- for (i = 0, size = 0; i < maxlen; i++)
+- if (str[i] != ' ')
+- size = i + 1;
+-
+- return size;
+-}
+-
+-static bool sfp_match(const char *qs, const char *str, size_t len)
+-{
+- if (!qs)
+- return true;
+- if (strlen(qs) != len)
+- return false;
+- return !strncmp(qs, str, len);
+-}
+-
+-static const struct sfp_quirk *sfp_lookup_quirk(const struct sfp_eeprom_id *id)
+-{
+- const struct sfp_quirk *q;
+- unsigned int i;
+- size_t vs, ps;
+-
+- vs = sfp_strlen(id->base.vendor_name, ARRAY_SIZE(id->base.vendor_name));
+- ps = sfp_strlen(id->base.vendor_pn, ARRAY_SIZE(id->base.vendor_pn));
+-
+- for (i = 0, q = sfp_quirks; i < ARRAY_SIZE(sfp_quirks); i++, q++)
+- if (sfp_match(q->vendor, id->base.vendor_name, vs) &&
+- sfp_match(q->part, id->base.vendor_pn, ps))
+- return q;
+-
+- return NULL;
+-}
+-
+ /**
+ * sfp_parse_port() - Parse the EEPROM base ID, setting the port type
+ * @bus: a pointer to the &struct sfp_bus structure for the sfp module
+@@ -359,7 +272,7 @@ void sfp_parse_support(struct sfp_bus *bus, const struct sfp_eeprom_id *id,
+ phylink_set(modes, 1000baseX_Full);
+ }
+
+- if (bus->sfp_quirk)
++ if (bus->sfp_quirk && bus->sfp_quirk->modes)
+ bus->sfp_quirk->modes(id, modes);
+
+ bitmap_or(support, support, modes, __ETHTOOL_LINK_MODE_MASK_NBITS);
+@@ -734,12 +647,13 @@ void sfp_link_down(struct sfp_bus *bus)
+ }
+ EXPORT_SYMBOL_GPL(sfp_link_down);
+
+-int sfp_module_insert(struct sfp_bus *bus, const struct sfp_eeprom_id *id)
++int sfp_module_insert(struct sfp_bus *bus, const struct sfp_eeprom_id *id,
++ const struct sfp_quirk *quirk)
+ {
+ const struct sfp_upstream_ops *ops = sfp_get_upstream_ops(bus);
+ int ret = 0;
+
+- bus->sfp_quirk = sfp_lookup_quirk(id);
++ bus->sfp_quirk = quirk;
+
+ if (ops && ops->module_insert)
+ ret = ops->module_insert(bus->upstream, id);
+diff --git a/drivers/net/phy/sfp.c b/drivers/net/phy/sfp.c
+index 3253366..8d95f49 100644
+--- a/drivers/net/phy/sfp.c
++++ b/drivers/net/phy/sfp.c
+@@ -165,6 +165,7 @@ static const enum gpiod_flags gpio_flags[] = {
+ * on board (for a copper SFP) time to initialise.
+ */
+ #define T_WAIT msecs_to_jiffies(50)
++#define T_WAIT_ROLLBALL msecs_to_jiffies(25000)
+ #define T_START_UP msecs_to_jiffies(300)
+ #define T_START_UP_BAD_GPON msecs_to_jiffies(60000)
+
+@@ -204,8 +205,11 @@ static const enum gpiod_flags gpio_flags[] = {
+
+ /* SFP modules appear to always have their PHY configured for bus address
+ * 0x56 (which with mdio-i2c, translates to a PHY address of 22).
++ * RollBall SFPs access phy via SFP Enhanced Digital Diagnostic Interface
++ * via address 0x51 (mdio-i2c will use RollBall protocol on this address).
+ */
+-#define SFP_PHY_ADDR 22
++#define SFP_PHY_ADDR 22
++#define SFP_PHY_ADDR_ROLLBALL 17
+
+ struct sff_data {
+ unsigned int gpios;
+@@ -217,6 +221,7 @@ struct sfp {
+ struct i2c_adapter *i2c;
+ struct mii_bus *i2c_mii;
+ struct sfp_bus *sfp_bus;
++ enum mdio_i2c_proto mdio_protocol;
+ struct phy_device *mod_phy;
+ const struct sff_data *type;
+ size_t i2c_block_size;
+@@ -233,6 +238,7 @@ struct sfp {
+ bool need_poll;
+
+ struct mutex st_mutex; /* Protects state */
++ unsigned int state_hw_mask;
+ unsigned int state_soft_mask;
+ unsigned int state;
+ struct delayed_work poll;
+@@ -249,6 +255,10 @@ struct sfp {
+ struct sfp_eeprom_id id;
+ unsigned int module_power_mW;
+ unsigned int module_t_start_up;
++ unsigned int module_t_wait;
++ bool tx_fault_ignore;
++
++ const struct sfp_quirk *quirk;
+
+ #if IS_ENABLED(CONFIG_HWMON)
+ struct sfp_diag diag;
+@@ -303,6 +313,135 @@ static const struct of_device_id sfp_of_match[] = {
+ };
+ MODULE_DEVICE_TABLE(of, sfp_of_match);
+
++static void sfp_fixup_long_startup(struct sfp *sfp)
++{
++ sfp->module_t_start_up = T_START_UP_BAD_GPON;
++}
++
++static void sfp_fixup_ignore_tx_fault(struct sfp *sfp)
++{
++ sfp->tx_fault_ignore = true;
++}
++
++static void sfp_fixup_halny_gsfp(struct sfp *sfp)
++{
++ /* Ignore the TX_FAULT and LOS signals on this module.
++ * these are possibly used for other purposes on this
++ * module, e.g. a serial port.
++ */
++ sfp->state_hw_mask &= ~(SFP_F_TX_FAULT | SFP_F_LOS);
++}
++
++static void sfp_fixup_rollball(struct sfp *sfp)
++{
++ sfp->mdio_protocol = MDIO_I2C_ROLLBALL;
++ sfp->module_t_wait = T_WAIT_ROLLBALL;
++}
++
++static void sfp_fixup_rollball_cc(struct sfp *sfp)
++{
++ sfp_fixup_rollball(sfp);
++
++ /* Some RollBall SFPs may have wrong (zero) extended compliance code
++ * burned in EEPROM. For PHY probing we need the correct one.
++ */
++ sfp->id.base.extended_cc = SFF8024_ECC_10GBASE_T_SFI;
++}
++
++static void sfp_quirk_2500basex(const struct sfp_eeprom_id *id,
++ unsigned long *modes)
++{
++ linkmode_set_bit(ETHTOOL_LINK_MODE_2500baseX_Full_BIT, modes);
++}
++
++static void sfp_quirk_ubnt_uf_instant(const struct sfp_eeprom_id *id,
++ unsigned long *modes)
++{
++ /* Ubiquiti U-Fiber Instant module claims that support all transceiver
++ * types including 10G Ethernet which is not truth. So clear all claimed
++ * modes and set only one mode which module supports: 1000baseX_Full.
++ */
++ linkmode_zero(modes);
++ linkmode_set_bit(ETHTOOL_LINK_MODE_1000baseX_Full_BIT, modes);
++}
++
++#define SFP_QUIRK(_v, _p, _m, _f) \
++ { .vendor = _v, .part = _p, .modes = _m, .fixup = _f, }
++#define SFP_QUIRK_M(_v, _p, _m) SFP_QUIRK(_v, _p, _m, NULL)
++#define SFP_QUIRK_F(_v, _p, _f) SFP_QUIRK(_v, _p, NULL, _f)
++
++static const struct sfp_quirk sfp_quirks[] = {
++ // Alcatel Lucent G-010S-P can operate at 2500base-X, but incorrectly
++ // report 2500MBd NRZ in their EEPROM
++ SFP_QUIRK_M("ALCATELLUCENT", "G010SP", sfp_quirk_2500basex),
++
++ // Alcatel Lucent G-010S-A can operate at 2500base-X, but report 3.2GBd
++ // NRZ in their EEPROM
++ SFP_QUIRK("ALCATELLUCENT", "3FE46541AA", sfp_quirk_2500basex,
++ sfp_fixup_long_startup),
++
++ SFP_QUIRK_F("HALNy", "HL-GSFP", sfp_fixup_halny_gsfp),
++
++ // Huawei MA5671A can operate at 2500base-X, but report 1.2GBd NRZ in
++ // their EEPROM
++ SFP_QUIRK("HUAWEI", "MA5671A", sfp_quirk_2500basex,
++ sfp_fixup_ignore_tx_fault),
++
++ // Lantech 8330-262D-E can operate at 2500base-X, but incorrectly report
++ // 2500MBd NRZ in their EEPROM
++ SFP_QUIRK_M("Lantech", "8330-262D-E", sfp_quirk_2500basex),
++
++ SFP_QUIRK_M("UBNT", "UF-INSTANT", sfp_quirk_ubnt_uf_instant),
++
++ SFP_QUIRK_F("OEM", "SFP-10G-T", sfp_fixup_rollball_cc),
++ SFP_QUIRK_F("OEM", "RTSFP-10", sfp_fixup_rollball_cc),
++ SFP_QUIRK_F("OEM", "RTSFP-10G", sfp_fixup_rollball_cc),
++ SFP_QUIRK_F("OEM", "TNBYV02-C0X-C3", sfp_fixup_rollball_cc),
++ SFP_QUIRK_F("Turris", "RTSFP-10", sfp_fixup_rollball),
++ SFP_QUIRK_F("Turris", "RTSFP-10G", sfp_fixup_rollball),
++ SFP_QUIRK_F("JESS-LINK", "P60000BBC001-1", sfp_fixup_rollball),
++};
++
++static size_t sfp_strlen(const char *str, size_t maxlen)
++{
++ size_t size, i;
++
++ /* Trailing characters should be filled with space chars, but
++ * some manufacturers can't read SFF-8472 and use NUL.
++ */
++ for (i = 0, size = 0; i < maxlen; i++)
++ if (str[i] != ' ' && str[i] != '\0')
++ size = i + 1;
++
++ return size;
++}
++
++static bool sfp_match(const char *qs, const char *str, size_t len)
++{
++ if (!qs)
++ return true;
++ if (strlen(qs) != len)
++ return false;
++ return !strncmp(qs, str, len);
++}
++
++static const struct sfp_quirk *sfp_lookup_quirk(const struct sfp_eeprom_id *id)
++{
++ const struct sfp_quirk *q;
++ unsigned int i;
++ size_t vs, ps;
++
++ vs = sfp_strlen(id->base.vendor_name, ARRAY_SIZE(id->base.vendor_name));
++ ps = sfp_strlen(id->base.vendor_pn, ARRAY_SIZE(id->base.vendor_pn));
++
++ for (i = 0, q = sfp_quirks; i < ARRAY_SIZE(sfp_quirks); i++, q++)
++ if (sfp_match(q->vendor, id->base.vendor_name, vs) &&
++ sfp_match(q->part, id->base.vendor_pn, ps))
++ return q;
++
++ return NULL;
++}
++
+ static unsigned long poll_jiffies;
+
+ static unsigned int sfp_gpio_get_state(struct sfp *sfp)
+@@ -414,9 +553,6 @@ static int sfp_i2c_write(struct sfp *sfp, bool a2, u8 dev_addr, void *buf,
+
+ static int sfp_i2c_configure(struct sfp *sfp, struct i2c_adapter *i2c)
+ {
+- struct mii_bus *i2c_mii;
+- int ret;
+-
+ if (!i2c_check_functionality(i2c, I2C_FUNC_I2C))
+ return -EINVAL;
+
+@@ -424,7 +560,15 @@ static int sfp_i2c_configure(struct sfp *sfp, struct i2c_adapter *i2c)
+ sfp->read = sfp_i2c_read;
+ sfp->write = sfp_i2c_write;
+
+- i2c_mii = mdio_i2c_alloc(sfp->dev, i2c);
++ return 0;
++}
++
++static int sfp_i2c_mdiobus_create(struct sfp *sfp)
++{
++ struct mii_bus *i2c_mii;
++ int ret;
++
++ i2c_mii = mdio_i2c_alloc(sfp->dev, sfp->i2c, sfp->mdio_protocol);
+ if (IS_ERR(i2c_mii))
+ return PTR_ERR(i2c_mii);
+
+@@ -442,6 +586,12 @@ static int sfp_i2c_configure(struct sfp *sfp, struct i2c_adapter *i2c)
+ return 0;
+ }
+
++static void sfp_i2c_mdiobus_destroy(struct sfp *sfp)
++{
++ mdiobus_unregister(sfp->i2c_mii);
++ sfp->i2c_mii = NULL;
++}
++
+ /* Interface */
+ static int sfp_read(struct sfp *sfp, bool a2, u8 addr, void *buf, size_t len)
+ {
+@@ -487,17 +637,18 @@ static void sfp_soft_set_state(struct sfp *sfp, unsigned int state)
+ static void sfp_soft_start_poll(struct sfp *sfp)
+ {
+ const struct sfp_eeprom_id *id = &sfp->id;
++ unsigned int mask = 0;
+
+ sfp->state_soft_mask = 0;
+- if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_DISABLE &&
+- !sfp->gpio[GPIO_TX_DISABLE])
+- sfp->state_soft_mask |= SFP_F_TX_DISABLE;
+- if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_FAULT &&
+- !sfp->gpio[GPIO_TX_FAULT])
+- sfp->state_soft_mask |= SFP_F_TX_FAULT;
+- if (id->ext.enhopts & SFP_ENHOPTS_SOFT_RX_LOS &&
+- !sfp->gpio[GPIO_LOS])
+- sfp->state_soft_mask |= SFP_F_LOS;
++ if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_DISABLE)
++ mask |= SFP_F_TX_DISABLE;
++ if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_FAULT)
++ mask |= SFP_F_TX_FAULT;
++ if (id->ext.enhopts & SFP_ENHOPTS_SOFT_RX_LOS)
++ mask |= SFP_F_LOS;
++
++ // Poll the soft state for hardware pins we want to ignore
++ sfp->state_soft_mask = ~sfp->state_hw_mask & mask;
+
+ if (sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT) &&
+ !sfp->need_poll)
+@@ -511,10 +662,11 @@ static void sfp_soft_stop_poll(struct sfp *sfp)
+
+ static unsigned int sfp_get_state(struct sfp *sfp)
+ {
+- unsigned int state = sfp->get_state(sfp);
++ unsigned int soft = sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT);
++ unsigned int state;
+
+- if (state & SFP_F_PRESENT &&
+- sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT))
++ state = sfp->get_state(sfp) & sfp->state_hw_mask;
++ if (state & SFP_F_PRESENT && soft)
+ state |= sfp_soft_get_state(sfp);
+
+ return state;
+@@ -1448,12 +1600,12 @@ static void sfp_sm_phy_detach(struct sfp *sfp)
+ sfp->mod_phy = NULL;
+ }
+
+-static int sfp_sm_probe_phy(struct sfp *sfp, bool is_c45)
++static int sfp_sm_probe_phy(struct sfp *sfp, int addr, bool is_c45)
+ {
+ struct phy_device *phy;
+ int err;
+
+- phy = get_phy_device(sfp->i2c_mii, SFP_PHY_ADDR, is_c45);
++ phy = get_phy_device(sfp->i2c_mii, addr, is_c45);
+ if (phy == ERR_PTR(-ENODEV))
+ return PTR_ERR(phy);
+ if (IS_ERR(phy)) {
+@@ -1548,6 +1700,14 @@ static void sfp_sm_fault(struct sfp *sfp, unsigned int next_state, bool warn)
+ }
+ }
+
++static int sfp_sm_add_mdio_bus(struct sfp *sfp)
++{
++ if (sfp->mdio_protocol != MDIO_I2C_NONE)
++ return sfp_i2c_mdiobus_create(sfp);
++
++ return 0;
++}
++
+ /* Probe a SFP for a PHY device if the module supports copper - the PHY
+ * normally sits at I2C bus address 0x56, and may either be a clause 22
+ * or clause 45 PHY.
+@@ -1563,19 +1723,23 @@ static int sfp_sm_probe_for_phy(struct sfp *sfp)
+ {
+ int err = 0;
+
+- switch (sfp->id.base.extended_cc) {
+- case SFF8024_ECC_10GBASE_T_SFI:
+- case SFF8024_ECC_10GBASE_T_SR:
+- case SFF8024_ECC_5GBASE_T:
+- case SFF8024_ECC_2_5GBASE_T:
+- err = sfp_sm_probe_phy(sfp, true);
++ switch (sfp->mdio_protocol) {
++ case MDIO_I2C_NONE:
+ break;
+
+- default:
+- if (sfp->id.base.e1000_base_t)
+- err = sfp_sm_probe_phy(sfp, false);
++ case MDIO_I2C_MARVELL_C22:
++ err = sfp_sm_probe_phy(sfp, SFP_PHY_ADDR, false);
++ break;
++
++ case MDIO_I2C_C45:
++ err = sfp_sm_probe_phy(sfp, SFP_PHY_ADDR, true);
++ break;
++
++ case MDIO_I2C_ROLLBALL:
++ err = sfp_sm_probe_phy(sfp, SFP_PHY_ADDR_ROLLBALL, true);
+ break;
+ }
++
+ return err;
+ }
+
+@@ -1819,11 +1983,33 @@ static int sfp_sm_mod_probe(struct sfp *sfp, bool report)
+ if (ret < 0)
+ return ret;
+
+- if (!memcmp(id.base.vendor_name, "ALCATELLUCENT ", 16) &&
+- !memcmp(id.base.vendor_pn, "3FE46541AA ", 16))
+- sfp->module_t_start_up = T_START_UP_BAD_GPON;
++ /* Initialise state bits to use from hardware */
++ sfp->state_hw_mask = SFP_F_PRESENT;
++ if (sfp->gpio[GPIO_TX_DISABLE])
++ sfp->state_hw_mask |= SFP_F_TX_DISABLE;
++ if (sfp->gpio[GPIO_TX_FAULT])
++ sfp->state_hw_mask |= SFP_F_TX_FAULT;
++ if (sfp->gpio[GPIO_LOS])
++ sfp->state_hw_mask |= SFP_F_LOS;
++
++ sfp->module_t_start_up = T_START_UP;
++ sfp->module_t_wait = T_WAIT;
++
++ sfp->tx_fault_ignore = false;
++
++ if (sfp->id.base.extended_cc == SFF8024_ECC_10GBASE_T_SFI ||
++ sfp->id.base.extended_cc == SFF8024_ECC_10GBASE_T_SR ||
++ sfp->id.base.extended_cc == SFF8024_ECC_5GBASE_T ||
++ sfp->id.base.extended_cc == SFF8024_ECC_2_5GBASE_T)
++ sfp->mdio_protocol = MDIO_I2C_C45;
++ else if (sfp->id.base.e1000_base_t)
++ sfp->mdio_protocol = MDIO_I2C_MARVELL_C22;
+ else
+- sfp->module_t_start_up = T_START_UP;
++ sfp->mdio_protocol = MDIO_I2C_NONE;
++
++ sfp->quirk = sfp_lookup_quirk(&id);
++ if (sfp->quirk && sfp->quirk->fixup)
++ sfp->quirk->fixup(sfp);
+
+ return 0;
+ }
+@@ -1936,7 +2122,8 @@ static void sfp_sm_module(struct sfp *sfp, unsigned int event)
+ break;
+
+ /* Report the module insertion to the upstream device */
+- err = sfp_module_insert(sfp->sfp_bus, &sfp->id);
++ err = sfp_module_insert(sfp->sfp_bus, &sfp->id,
++ sfp->quirk);
+ if (err < 0) {
+ sfp_sm_mod_next(sfp, SFP_MOD_ERROR, 0);
+ break;
+@@ -1995,6 +2182,8 @@ static void sfp_sm_main(struct sfp *sfp, unsigned int event)
+ sfp_module_stop(sfp->sfp_bus);
+ if (sfp->mod_phy)
+ sfp_sm_phy_detach(sfp);
++ if (sfp->i2c_mii)
++ sfp_i2c_mdiobus_destroy(sfp);
+ sfp_module_tx_disable(sfp);
+ sfp_soft_stop_poll(sfp);
+ sfp_sm_next(sfp, SFP_S_DOWN, 0);
+@@ -2018,9 +2207,10 @@ static void sfp_sm_main(struct sfp *sfp, unsigned int event)
+
+ /* We need to check the TX_FAULT state, which is not defined
+ * while TX_DISABLE is asserted. The earliest we want to do
+- * anything (such as probe for a PHY) is 50ms.
++ * anything (such as probe for a PHY) is 50ms (or more on
++ * specific modules).
+ */
+- sfp_sm_next(sfp, SFP_S_WAIT, T_WAIT);
++ sfp_sm_next(sfp, SFP_S_WAIT, sfp->module_t_wait);
+ break;
+
+ case SFP_S_WAIT:
+@@ -2034,8 +2224,8 @@ static void sfp_sm_main(struct sfp *sfp, unsigned int event)
+ * deasserting.
+ */
+ timeout = sfp->module_t_start_up;
+- if (timeout > T_WAIT)
+- timeout -= T_WAIT;
++ if (timeout > sfp->module_t_wait)
++ timeout -= sfp->module_t_wait;
+ else
+ timeout = 1;
+
+@@ -2057,6 +2247,12 @@ static void sfp_sm_main(struct sfp *sfp, unsigned int event)
+ sfp->sm_fault_retries == N_FAULT_INIT);
+ } else if (event == SFP_E_TIMEOUT || event == SFP_E_TX_CLEAR) {
+ init_done:
++ /* Create mdiobus and start trying for PHY */
++ ret = sfp_sm_add_mdio_bus(sfp);
++ if (ret < 0) {
++ sfp_sm_next(sfp, SFP_S_FAIL, 0);
++ break;
++ }
+ sfp->sm_phy_retries = R_PHY_RETRY;
+ goto phy_probe;
+ }
+@@ -2409,6 +2605,8 @@ static int sfp_probe(struct platform_device *pdev)
+ return PTR_ERR(sfp->gpio[i]);
+ }
+
++ sfp->state_hw_mask = SFP_F_PRESENT;
++
+ sfp->get_state = sfp_gpio_get_state;
+ sfp->set_state = sfp_gpio_set_state;
+
+diff --git a/drivers/net/phy/sfp.h b/drivers/net/phy/sfp.h
+index b83f705..ef06d35 100644
+--- a/drivers/net/phy/sfp.h
++++ b/drivers/net/phy/sfp.h
+@@ -6,6 +6,13 @@
+
+ struct sfp;
+
++struct sfp_quirk {
++ const char *vendor;
++ const char *part;
++ void (*modes)(const struct sfp_eeprom_id *id, unsigned long *modes);
++ void (*fixup)(struct sfp *sfp);
++};
++
+ struct sfp_socket_ops {
+ void (*attach)(struct sfp *sfp);
+ void (*detach)(struct sfp *sfp);
+@@ -20,7 +27,8 @@ int sfp_add_phy(struct sfp_bus *bus, struct phy_device *phydev);
+ void sfp_remove_phy(struct sfp_bus *bus);
+ void sfp_link_up(struct sfp_bus *bus);
+ void sfp_link_down(struct sfp_bus *bus);
+-int sfp_module_insert(struct sfp_bus *bus, const struct sfp_eeprom_id *id);
++int sfp_module_insert(struct sfp_bus *bus, const struct sfp_eeprom_id *id,
++ const struct sfp_quirk *quirk);
+ void sfp_module_remove(struct sfp_bus *bus);
+ int sfp_module_start(struct sfp_bus *bus);
+ void sfp_module_stop(struct sfp_bus *bus);
+diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
+index 512f27b..2c2d638 100644
+--- a/drivers/net/phy/marvell10g.c
++++ b/drivers/net/phy/marvell10g.c
+@@ -155,13 +155,6 @@ static int mv3310_hwmon_config(struct phy_device *phydev, bool enable)
+ MV_V2_TEMP_CTRL_MASK, val);
+ }
+
+-static void mv3310_hwmon_disable(void *data)
+-{
+- struct phy_device *phydev = data;
+-
+- mv3310_hwmon_config(phydev, false);
+-}
+-
+ static int mv3310_hwmon_probe(struct phy_device *phydev)
+ {
+ struct device *dev = &phydev->mdio.dev;
+@@ -185,10 +178,6 @@ static int mv3310_hwmon_probe(struct phy_device *phydev)
+ if (ret)
+ return ret;
+
+- ret = devm_add_action_or_reset(dev, mv3310_hwmon_disable, phydev);
+- if (ret)
+- return ret;
+-
+ priv->hwmon_dev = devm_hwmon_device_register_with_info(dev,
+ priv->hwmon_name, phydev,
+ &mv3310_hwmon_chip_info, NULL);
+@@ -262,6 +251,11 @@ static int mv3310_probe(struct phy_device *phydev)
+ return phy_sfp_probe(phydev, &mv3310_sfp_ops);
+ }
+
++static void mv3310_remove(struct phy_device *phydev)
++{
++ mv3310_hwmon_config(phydev, false);
++}
++
+ static int mv3310_suspend(struct phy_device *phydev)
+ {
+ return phy_set_bits_mmd(phydev, MDIO_MMD_VEND2, MV_V2_PORT_CTRL,
+@@ -513,6 +507,7 @@ static struct phy_driver mv3310_drivers[] = {
+ .config_aneg = mv3310_config_aneg,
+ .aneg_done = mv3310_aneg_done,
+ .read_status = mv3310_read_status,
++ .remove = mv3310_remove,
+ },
+ {
+ .phy_id = MARVELL_PHY_ID_88E2110,
+@@ -526,6 +521,7 @@ static struct phy_driver mv3310_drivers[] = {
+ .config_aneg = mv3310_config_aneg,
+ .aneg_done = mv3310_aneg_done,
+ .read_status = mv3310_read_status,
++ .remove = mv3310_remove,
+ },
+ };
+