sky2: add support for read/write of EEPROM

Add get/set eeprom support for sky2.
Signed-off-by: Stephen Hemminger <shemminger@linux-foundation.org>
Signed-off-by: Jeff Garzik <jeff@garzik.org>

drivers/net/sky2.c

@@ -77,6 +77,9 @@
 #define NAPI_WEIGHT		64
 #define PHY_RETRIES		1000
 
+#define SKY2_EEPROM_MAGIC	0x9955aabb
+
+
 #define RING_NEXT(x,s)	(((x)+1) & ((s)-1))
 
 static const u32 default_msg =
@@ -3429,6 +3432,90 @@ static int sky2_set_tso(struct net_device *dev, u32 data)
 	return ethtool_op_set_tso(dev, data);
 }
 
+static int sky2_get_eeprom_len(struct net_device *dev)
+{
+	struct sky2_port *sky2 = netdev_priv(dev);
+	u16 reg2;
+
+	reg2 = sky2_pci_read32(sky2->hw, PCI_DEV_REG2);
+	return 1 << ( ((reg2 & PCI_VPD_ROM_SZ) >> 14) + 8);
+}
+
+static u32 sky2_vpd_read(struct sky2_hw *hw, int cap, u16 offset)
+{
+	sky2_pci_write16(hw, cap + PCI_VPD_ADDR, offset);
+
+	while (!(sky2_pci_read16(hw, cap + PCI_VPD_ADDR) & PCI_VPD_ADDR_F))
+			cpu_relax();
+	return sky2_pci_read32(hw, cap + PCI_VPD_DATA);
+}
+
+static void sky2_vpd_write(struct sky2_hw *hw, int cap, u16 offset, u32 val)
+{
+	sky2_pci_write32(hw, cap + PCI_VPD_DATA, val);
+	sky2_pci_write16(hw, cap + PCI_VPD_ADDR, offset | PCI_VPD_ADDR_F);
+	do {
+		cpu_relax();
+	} while (sky2_pci_read16(hw, cap + PCI_VPD_ADDR) & PCI_VPD_ADDR_F);
+}
+
+static int sky2_get_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
+			   u8 *data)
+{
+	struct sky2_port *sky2 = netdev_priv(dev);
+	int cap = pci_find_capability(sky2->hw->pdev, PCI_CAP_ID_VPD);
+	int length = eeprom->len;
+	u16 offset = eeprom->offset;
+
+	if (!cap)
+		return -EINVAL;
+
+	eeprom->magic = SKY2_EEPROM_MAGIC;
+
+	while (length > 0) {
+		u32 val = sky2_vpd_read(sky2->hw, cap, offset);
+		int n = min_t(int, length, sizeof(val));
+
+		memcpy(data, &val, n);
+		length -= n;
+		data += n;
+		offset += n;
+	}
+	return 0;
+}
+
+static int sky2_set_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
+			   u8 *data)
+{
+	struct sky2_port *sky2 = netdev_priv(dev);
+	int cap = pci_find_capability(sky2->hw->pdev, PCI_CAP_ID_VPD);
+	int length = eeprom->len;
+	u16 offset = eeprom->offset;
+
+	if (!cap)
+		return -EINVAL;
+
+	if (eeprom->magic != SKY2_EEPROM_MAGIC)
+		return -EINVAL;
+
+	while (length > 0) {
+		u32 val;
+		int n = min_t(int, length, sizeof(val));
+
+		if (n < sizeof(val))
+			val = sky2_vpd_read(sky2->hw, cap, offset);
+		memcpy(&val, data, n);
+
+		sky2_vpd_write(sky2->hw, cap, offset, val);
+
+		length -= n;
+		data += n;
+		offset += n;
+	}
+	return 0;
+}
+
+
 static const struct ethtool_ops sky2_ethtool_ops = {
 	.get_settings	= sky2_get_settings,
 	.set_settings	= sky2_set_settings,
@@ -3441,6 +3528,9 @@ static const struct ethtool_ops sky2_ethtool_ops = {
 	.get_regs_len	= sky2_get_regs_len,
 	.get_regs	= sky2_get_regs,
 	.get_link	= ethtool_op_get_link,
+	.get_eeprom_len	= sky2_get_eeprom_len,
+	.get_eeprom	= sky2_get_eeprom,
+	.set_eeprom	= sky2_set_eeprom,
 	.get_sg 	= ethtool_op_get_sg,
 	.set_sg 	= ethtool_op_set_sg,
 	.get_tx_csum	= ethtool_op_get_tx_csum,