diff --git a/drivers/net/phy/sfp.c b/drivers/net/phy/sfp.c index caeda3291b30..03bfd8640db9 100644 --- a/drivers/net/phy/sfp.c +++ b/drivers/net/phy/sfp.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #include "sfp.h" @@ -776,50 +777,113 @@ static int sfp_i2c_write(struct sfp *sfp, bool a2, u8 dev_addr, void *buf, return ret == ARRAY_SIZE(msgs) ? len : 0; } -static int sfp_smbus_byte_read(struct sfp *sfp, bool a2, u8 dev_addr, - void *buf, size_t len) +static int sfp_smbus_read(struct sfp *sfp, bool a2, u8 dev_addr, void *buf, + size_t len) { - union i2c_smbus_data smbus_data; + union i2c_smbus_data smbus_data = {0}; u8 bus_addr = a2 ? 0x51 : 0x50; + size_t this_len, transferred; + u32 functionality; u8 *data = buf; int ret; + functionality = i2c_get_functionality(sfp->i2c); + while (len) { - ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, - I2C_SMBUS_READ, dev_addr, - I2C_SMBUS_BYTE_DATA, &smbus_data); - if (ret < 0) - return ret; + this_len = min(len, sfp->i2c_block_size); - *data = smbus_data.byte; + if (functionality & I2C_FUNC_SMBUS_READ_I2C_BLOCK) { + smbus_data.block[0] = this_len; + ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, + I2C_SMBUS_READ, dev_addr, + I2C_SMBUS_I2C_BLOCK_DATA, &smbus_data); + if (ret < 0) + return ret; - len--; - data++; - dev_addr++; + transferred = min_t(size_t, smbus_data.block[0], this_len); + if (!transferred) + return -EIO; + + memcpy(data, &smbus_data.block[1], transferred); + } else if (this_len >= 2 && + (functionality & I2C_FUNC_SMBUS_READ_WORD_DATA)) { + ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, + I2C_SMBUS_READ, dev_addr, + I2C_SMBUS_WORD_DATA, &smbus_data); + if (ret < 0) + return ret; + + put_unaligned_le16(smbus_data.word, data); + transferred = 2; + } else { + ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, + I2C_SMBUS_READ, dev_addr, + I2C_SMBUS_BYTE_DATA, &smbus_data); + if (ret < 0) + return ret; + + *data = smbus_data.byte; + transferred = 1; + } + + data += transferred; + len -= transferred; + dev_addr += transferred; } return data - (u8 *)buf; } -static int sfp_smbus_byte_write(struct sfp *sfp, bool a2, u8 dev_addr, - void *buf, size_t len) +static int sfp_smbus_write(struct sfp *sfp, bool a2, u8 dev_addr, void *buf, + size_t len) { union i2c_smbus_data smbus_data; u8 bus_addr = a2 ? 0x51 : 0x50; + size_t this_len, transferred; + u32 functionality; u8 *data = buf; int ret; - while (len) { - smbus_data.byte = *data; - ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, - I2C_SMBUS_WRITE, dev_addr, - I2C_SMBUS_BYTE_DATA, &smbus_data); - if (ret) - return ret; + functionality = i2c_get_functionality(sfp->i2c); - len--; - data++; - dev_addr++; + while (len) { + this_len = min(len, sfp->i2c_block_size); + + if (functionality & I2C_FUNC_SMBUS_WRITE_I2C_BLOCK) { + smbus_data.block[0] = this_len; + memcpy(&smbus_data.block[1], data, this_len); + + ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, + I2C_SMBUS_WRITE, dev_addr, + I2C_SMBUS_I2C_BLOCK_DATA, &smbus_data); + if (ret < 0) + return ret; + + transferred = this_len; + } else if (this_len >= 2 && + (functionality & I2C_FUNC_SMBUS_WRITE_WORD_DATA)) { + smbus_data.word = get_unaligned_le16(data); + ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, + I2C_SMBUS_WRITE, dev_addr, + I2C_SMBUS_WORD_DATA, &smbus_data); + if (ret < 0) + return ret; + + transferred = 2; + } else { + smbus_data.byte = *data; + ret = i2c_smbus_xfer(sfp->i2c, bus_addr, 0, + I2C_SMBUS_WRITE, dev_addr, + I2C_SMBUS_BYTE_DATA, &smbus_data); + if (ret < 0) + return ret; + + transferred = 1; + } + + data += transferred; + len -= transferred; + dev_addr += transferred; } return data - (u8 *)buf; @@ -827,21 +891,48 @@ static int sfp_smbus_byte_write(struct sfp *sfp, bool a2, u8 dev_addr, static int sfp_i2c_configure(struct sfp *sfp, struct i2c_adapter *i2c) { + size_t max_block_size; + sfp->i2c = i2c; if (i2c_check_functionality(i2c, I2C_FUNC_I2C)) { sfp->read = sfp_i2c_read; sfp->write = sfp_i2c_write; - sfp->i2c_max_block_size = SFP_EEPROM_BLOCK_SIZE; - } else if (i2c_check_functionality(i2c, I2C_FUNC_SMBUS_BYTE_DATA)) { - sfp->read = sfp_smbus_byte_read; - sfp->write = sfp_smbus_byte_write; - sfp->i2c_max_block_size = 1; + max_block_size = SFP_EEPROM_BLOCK_SIZE; + } else if (i2c_check_functionality(i2c, I2C_FUNC_SMBUS_BYTE_DATA) || + i2c_check_functionality(i2c, I2C_FUNC_SMBUS_I2C_BLOCK)) { + /* Either protocol alone covers any length: I2C-block carries + * 1..32 bytes per xfer, byte iterates one byte at a time. + */ + sfp->read = sfp_smbus_read; + sfp->write = sfp_smbus_write; + + if (i2c_check_functionality(i2c, I2C_FUNC_SMBUS_I2C_BLOCK)) + max_block_size = SFP_EEPROM_BLOCK_SIZE; + else if (i2c_check_functionality(i2c, I2C_FUNC_SMBUS_WORD_DATA)) + max_block_size = 2; + else + max_block_size = 1; + } else if (WARN_ONCE(i2c_check_functionality(i2c, I2C_FUNC_SMBUS_WORD_DATA), + "SMBus word-only adapter; odd-length transfers will fail\n")) { + /* Word-only: even-length xfers work; odd-length xfers fall + * to BYTE, which the adapter does not advertise and will + * likely fail. + */ + sfp->read = sfp_smbus_read; + sfp->write = sfp_smbus_write; + max_block_size = 2; } else { sfp->i2c = NULL; return -EINVAL; } + if (i2c->quirks && i2c->quirks->max_read_len) + max_block_size = min(max_block_size, i2c->quirks->max_read_len); + if (i2c->quirks && i2c->quirks->max_write_len) + max_block_size = min(max_block_size, i2c->quirks->max_write_len); + + sfp->i2c_max_block_size = max_block_size; sfp->i2c_block_size = sfp->i2c_max_block_size; return 0; }