[PATCH 2/7] net: e1000: properly read/write MDI registers on 8254x cards

[Denis Orlov <denorl2009@gmail.com>]

Taken from Linux e1000 driver.

Signed-off-by: Denis Orlov <denorl2009@gmail.com>
---
 drivers/net/e1000/e1000.h |  3 +++
 drivers/net/e1000/main.c  | 22 ++++++++++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/drivers/net/e1000/e1000.h b/drivers/net/e1000/e1000.h
index 52ad3d4cdb..3a5ee9988e 100644
--- a/drivers/net/e1000/e1000.h
+++ b/drivers/net/e1000/e1000.h
@@ -1388,6 +1388,9 @@ struct e1000_eeprom_info {
 #define PHY_1000T_STATUS		0x0A	/* 1000Base-T Status Reg */
 #define PHY_EXT_STATUS			0x0F	/* Extended Status Reg */
 
+#define MAX_PHY_REG_ADDRESS		0x1F	/* 5 bit address bus (0-0x1F) */
+#define MAX_PHY_MULTI_PAGE_REG		0xF	/* Registers equal on all pages */
+
 /* M88E1000 Specific Registers */
 #define M88E1000_PHY_SPEC_CTRL		0x10	/* PHY Specific Control Register */
 #define M88E1000_PHY_SPEC_STATUS	0x11	/* PHY Specific Status Register */
diff --git a/drivers/net/e1000/main.c b/drivers/net/e1000/main.c
index bfb8d8ff2f..3b45fb3b51 100644
--- a/drivers/net/e1000/main.c
+++ b/drivers/net/e1000/main.c
@@ -54,6 +54,8 @@ static int e1000_get_speed_and_duplex(struct e1000_hw *hw, uint16_t *speed,
 				       uint16_t *duplex);
 static int e1000_read_phy_reg(struct e1000_hw *hw, uint32_t reg_addr,
 			      uint16_t *phy_data);
+static int e1000_phy_write(struct mii_bus *bus, int phy_addr, int reg_addr,
+			   u16 phy_data);
 static int e1000_write_phy_reg(struct e1000_hw *hw, uint32_t reg_addr,
 			       uint16_t phy_data);
 static int32_t e1000_phy_hw_reset(struct e1000_hw *hw);
@@ -2627,6 +2629,15 @@ static int e1000_read_phy_reg(struct e1000_hw *hw, uint32_t reg_addr,
 {
 	int ret;
 
+	if ((hw->phy_type == e1000_phy_igp) && (reg_addr > MAX_PHY_MULTI_PAGE_REG)) {
+		ret = e1000_phy_write(&hw->miibus, 1, IGP01E1000_PHY_PAGE_SELECT,
+				      (u16)reg_addr);
+		if (ret)
+			return ret;
+
+		reg_addr &= MAX_PHY_REG_ADDRESS;
+	}
+
 	ret = e1000_phy_read(&hw->miibus, 1, reg_addr);
 	if (ret < 0)
 		return ret;
@@ -2702,6 +2713,17 @@ static int e1000_phy_write(struct mii_bus *bus, int phy_addr,
  ******************************************************************************/
 static int e1000_write_phy_reg(struct e1000_hw *hw, uint32_t reg_addr, uint16_t phy_data)
 {
+	int ret;
+
+	if ((hw->phy_type == e1000_phy_igp) && (reg_addr > MAX_PHY_MULTI_PAGE_REG)) {
+		ret = e1000_phy_write(&hw->miibus, 1, IGP01E1000_PHY_PAGE_SELECT,
+				      (u16)reg_addr);
+		if (ret)
+			return ret;
+
+		reg_addr &= MAX_PHY_REG_ADDRESS;
+	}
+
 	return e1000_phy_write(&hw->miibus, 1, reg_addr, phy_data);
 }
 
-- 
2.20.1