net/mlx5e: ethtool, Add support for EEPROM high pages query

[linux-2.6-microblaze.git] / drivers / net / ethernet / mellanox / mlx5 / core / port.c

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/port.c b/drivers/net/ethernet/mellanox/mlx5/core/port.c
index 361468e..cc262b3 100644 (file)
--- a/drivers/net/ethernet/mellanox/mlx5/core/port.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/port.c
@@ -293,15 +293,36 @@ static int mlx5_query_module_num(struct mlx5_core_dev *dev, int *module_num)
        return 0;
 }
 
+static int mlx5_eeprom_page(int offset)
+{
+       if (offset < MLX5_EEPROM_PAGE_LENGTH)
+               /* Addresses between 0-255 - page 00 */
+               return 0;
+
+       /* Addresses between 256 - 639 belongs to pages 01, 02 and 03
+        * For example, offset = 400 belongs to page 02:
+        * 1 + ((400 - 256)/128) = 2
+        */
+       return 1 + ((offset - MLX5_EEPROM_PAGE_LENGTH) /
+                   MLX5_EEPROM_HIGH_PAGE_LENGTH);
+}
+
+static int mlx5_eeprom_high_page_offset(int page_num)
+{
+       if (!page_num) /* Page 0 always start from low page */
+               return 0;
+
+       /* High page */
+       return page_num * MLX5_EEPROM_HIGH_PAGE_LENGTH;
+}
+
 int mlx5_query_module_eeprom(struct mlx5_core_dev *dev,
                             u16 offset, u16 size, u8 *data)
 {
+       int module_num, page_num, status, err;
        u32 out[MLX5_ST_SZ_DW(mcia_reg)];
        u32 in[MLX5_ST_SZ_DW(mcia_reg)];
-       int module_num;
        u16 i2c_addr;
-       int status;
-       int err;
        void *ptr = MLX5_ADDR_OF(mcia_reg, out, dword_0);
 
        err = mlx5_query_module_num(dev, &module_num);
@@ -311,8 +332,15 @@ int mlx5_query_module_eeprom(struct mlx5_core_dev *dev,
        memset(in, 0, sizeof(in));
        size = min_t(int, size, MLX5_EEPROM_MAX_BYTES);
 
-       if (offset < MLX5_EEPROM_PAGE_LENGTH &&
-           offset + size > MLX5_EEPROM_PAGE_LENGTH)
+       /* Get the page number related to the given offset */
+       page_num = mlx5_eeprom_page(offset);
+
+       /* Set the right offset according to the page number,
+        * For page_num > 0, relative offset is always >= 128 (high page).
+        */
+       offset -= mlx5_eeprom_high_page_offset(page_num);
+
+       if (offset + size > MLX5_EEPROM_PAGE_LENGTH)
                /* Cross pages read, read until offset 256 in low page */
                size -= offset + size - MLX5_EEPROM_PAGE_LENGTH;
 
@@ -321,7 +349,7 @@ int mlx5_query_module_eeprom(struct mlx5_core_dev *dev,
        MLX5_SET(mcia_reg, in, l, 0);
        MLX5_SET(mcia_reg, in, module, module_num);
        MLX5_SET(mcia_reg, in, i2c_device_address, i2c_addr);
-       MLX5_SET(mcia_reg, in, page_number, 0);
+       MLX5_SET(mcia_reg, in, page_number, page_num);
        MLX5_SET(mcia_reg, in, device_address, offset);
        MLX5_SET(mcia_reg, in, size, size);