[v1,net-next,6/9] lan743x: Add power management support

diff --git a/drivers/net/ethernet/microchip/lan743x_ethtool.c b/drivers/net/ethernet/microchip/lan743x_ethtool.c
index 0d0c997..0709a8d 100644
--- a/drivers/net/ethernet/microchip/lan743x_ethtool.c
+++ b/drivers/net/ethernet/microchip/lan743x_ethtool.c
@@ -417,6 +417,53 @@  static int lan743x_ethtool_get_sset_count(struct net_device *netdev, int sset)
 	}
 }
 
+#ifdef CONFIG_PM
+static void lan743x_ethtool_get_wol(struct net_device *netdev,
+				    struct ethtool_wolinfo *wol)
+{
+	struct lan743x_adapter *adapter = netdev_priv(netdev);
+	u32 data;
+
+	data = lan743x_csr_read(adapter, PMT_CTL);
+
+	wol->supported = WAKE_BCAST | WAKE_UCAST | WAKE_MCAST |
+		WAKE_MAGIC | WAKE_PHY | WAKE_ARP;
+
+	wol->wolopts = adapter->wolopts;
+}
+#endif /* CONFIG_PM */
+
+#if CONFIG_PM
+static int lan743x_ethtool_set_wol(struct net_device *netdev,
+				   struct ethtool_wolinfo *wol)
+{
+	struct lan743x_adapter *adapter = netdev_priv(netdev);
+
+	if (wol->wolopts & WAKE_MAGICSECURE)
+		return -EOPNOTSUPP;
+
+	adapter->wolopts = 0;
+	if (wol->wolopts & WAKE_UCAST)
+		adapter->wolopts |= WAKE_UCAST;
+	if (wol->wolopts & WAKE_MCAST)
+		adapter->wolopts |= WAKE_MCAST;
+	if (wol->wolopts & WAKE_BCAST)
+		adapter->wolopts |= WAKE_BCAST;
+	if (wol->wolopts & WAKE_MAGIC)
+		adapter->wolopts |= WAKE_MAGIC;
+	if (wol->wolopts & WAKE_PHY)
+		adapter->wolopts |= WAKE_PHY;
+	if (wol->wolopts & WAKE_ARP)
+		adapter->wolopts |= WAKE_ARP;
+
+	device_set_wakeup_enable(&adapter->pdev->dev, (bool)wol->wolopts);
+
+	phy_ethtool_set_wol(netdev->phydev, wol);
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
 const struct ethtool_ops lan743x_ethtool_ops = {
 	.get_drvinfo = lan743x_ethtool_get_drvinfo,
 	.get_msglevel = lan743x_ethtool_get_msglevel,
@@ -431,4 +478,8 @@  const struct ethtool_ops lan743x_ethtool_ops = {
 	.get_sset_count = lan743x_ethtool_get_sset_count,
 	.get_link_ksettings = phy_ethtool_get_link_ksettings,
 	.set_link_ksettings = phy_ethtool_set_link_ksettings,
+#ifdef CONFIG_PM
+	.get_wol = lan743x_ethtool_get_wol,
+	.set_wol = lan743x_ethtool_set_wol,
+#endif
 };
diff --git a/drivers/net/ethernet/microchip/lan743x_main.c b/drivers/net/ethernet/microchip/lan743x_main.c
index 1e2f8c6..52ca8b9 100644
--- a/drivers/net/ethernet/microchip/lan743x_main.c
+++ b/drivers/net/ethernet/microchip/lan743x_main.c
@@ -2749,10 +2749,217 @@  static void lan743x_pcidev_shutdown(struct pci_dev *pdev)
 		lan743x_netdev_close(netdev);
 	rtnl_unlock();
 
+#ifdef CONFIG_PM
+	pci_save_state(pdev);
+#endif
+
 	/* clean up lan743x portion */
 	lan743x_hardware_cleanup(adapter);
 }
 
+#ifdef CONFIG_PM
+static int lan743x_pm_wakeframe_crc16(const u8 *buf, int len)
+{
+	const u16 crc16poly = 0x8005;
+	u16 bit, crc, msb;
+	u8 data;
+	int i;
+
+	crc = 0xFFFF;
+	for (i = 0; i < len; i++) {
+		data = *buf++;
+		for (bit = 0; bit < 8; bit++) {
+			msb = crc >> 15;
+			crc <<= 1;
+
+			if (msb ^ (u16)(data & 1)) {
+				crc ^= crc16poly;
+				crc |= (u16)0x0001U;
+			}
+			data >>= 1;
+		}
+	}
+
+	return crc;
+}
+#endif /* CONFIG_PM */
+
+#if CONFIG_PM
+static void lan743x_pm_set_wol(struct lan743x_adapter *adapter)
+{
+	const u8 ipv4_multicast[3] = { 0x01, 0x00, 0x5E };
+	const u8 ipv6_multicast[3] = { 0x33, 0x33 };
+	const u8 arp_type[2] = { 0x08, 0x06 };
+	int mask_index;
+	u32 pmtctl;
+	u32 wucsr;
+	u32 macrx;
+	u16 crc;
+
+	for (mask_index = 0; mask_index < MAC_NUM_OF_WUF_CFG; mask_index++)
+		lan743x_csr_write(adapter, MAC_WUF_CFG(mask_index), 0);
+
+	/* clear wake settings */
+	pmtctl = lan743x_csr_read(adapter, PMT_CTL);
+	pmtctl |= PMT_CTL_WUPS_MASK_;
+	pmtctl &= ~(PMT_CTL_GPIO_WAKEUP_EN_ | PMT_CTL_EEE_WAKEUP_EN_ |
+		PMT_CTL_WOL_EN_ | PMT_CTL_MAC_D3_RX_CLK_OVR_ |
+		PMT_CTL_RX_FCT_RFE_D3_CLK_OVR_ | PMT_CTL_ETH_PHY_WAKE_EN_);
+
+	macrx = lan743x_csr_read(adapter, MAC_RX);
+
+	wucsr = 0;
+	mask_index = 0;
+
+	pmtctl |= PMT_CTL_ETH_PHY_D3_COLD_OVR_ | PMT_CTL_ETH_PHY_D3_OVR_;
+
+	if (adapter->wolopts & WAKE_PHY) {
+		pmtctl |= PMT_CTL_ETH_PHY_EDPD_PLL_CTL_;
+		pmtctl |= PMT_CTL_ETH_PHY_WAKE_EN_;
+	}
+	if (adapter->wolopts & WAKE_MAGIC) {
+		wucsr |= MAC_WUCSR_MPEN_;
+		macrx |= MAC_RX_RXEN_;
+		pmtctl |= PMT_CTL_WOL_EN_ | PMT_CTL_MAC_D3_RX_CLK_OVR_;
+	}
+	if (adapter->wolopts & WAKE_UCAST) {
+		wucsr |= MAC_WUCSR_RFE_WAKE_EN_ | MAC_WUCSR_PFDA_EN_;
+		macrx |= MAC_RX_RXEN_;
+		pmtctl |= PMT_CTL_WOL_EN_ | PMT_CTL_MAC_D3_RX_CLK_OVR_;
+		pmtctl |= PMT_CTL_RX_FCT_RFE_D3_CLK_OVR_;
+	}
+	if (adapter->wolopts & WAKE_BCAST) {
+		wucsr |= MAC_WUCSR_RFE_WAKE_EN_ | MAC_WUCSR_BCST_EN_;
+		macrx |= MAC_RX_RXEN_;
+		pmtctl |= PMT_CTL_WOL_EN_ | PMT_CTL_MAC_D3_RX_CLK_OVR_;
+		pmtctl |= PMT_CTL_RX_FCT_RFE_D3_CLK_OVR_;
+	}
+	if (adapter->wolopts & WAKE_MCAST) {
+		/* IPv4 multicast */
+		crc = lan743x_pm_wakeframe_crc16(ipv4_multicast, 3);
+		lan743x_csr_write(adapter, MAC_WUF_CFG(mask_index),
+				  MAC_WUF_CFG_EN_ | MAC_WUF_CFG_TYPE_MCAST_ |
+				  (0 << MAC_WUF_CFG_OFFSET_SHIFT_) |
+				  (crc & MAC_WUF_CFG_CRC16_MASK_));
+		lan743x_csr_write(adapter, MAC_WUF_MASK0(mask_index), 7);
+		lan743x_csr_write(adapter, MAC_WUF_MASK1(mask_index), 0);
+		lan743x_csr_write(adapter, MAC_WUF_MASK2(mask_index), 0);
+		lan743x_csr_write(adapter, MAC_WUF_MASK3(mask_index), 0);
+		mask_index++;
+
+		/* IPv6 multicast */
+		crc = lan743x_pm_wakeframe_crc16(ipv6_multicast, 2);
+		lan743x_csr_write(adapter, MAC_WUF_CFG(mask_index),
+				  MAC_WUF_CFG_EN_ | MAC_WUF_CFG_TYPE_MCAST_ |
+				  (0 << MAC_WUF_CFG_OFFSET_SHIFT_) |
+				  (crc & MAC_WUF_CFG_CRC16_MASK_));
+		lan743x_csr_write(adapter, MAC_WUF_MASK0(mask_index), 3);
+		lan743x_csr_write(adapter, MAC_WUF_MASK1(mask_index), 0);
+		lan743x_csr_write(adapter, MAC_WUF_MASK2(mask_index), 0);
+		lan743x_csr_write(adapter, MAC_WUF_MASK3(mask_index), 0);
+		mask_index++;
+
+		wucsr |= MAC_WUCSR_RFE_WAKE_EN_ | MAC_WUCSR_WAKE_EN_;
+		macrx |= MAC_RX_RXEN_;
+		pmtctl |= PMT_CTL_WOL_EN_ | PMT_CTL_MAC_D3_RX_CLK_OVR_;
+		pmtctl |= PMT_CTL_RX_FCT_RFE_D3_CLK_OVR_;
+	}
+	if (adapter->wolopts & WAKE_ARP) {
+		/* set MAC_WUF_CFG & WUF_MASK
+		 * for packettype (offset 12,13) = ARP (0x0806)
+		 */
+		crc = lan743x_pm_wakeframe_crc16(arp_type, 2);
+		lan743x_csr_write(adapter, MAC_WUF_CFG(mask_index),
+				  MAC_WUF_CFG_EN_ | MAC_WUF_CFG_TYPE_ALL_ |
+				  (0 << MAC_WUF_CFG_OFFSET_SHIFT_) |
+				  (crc & MAC_WUF_CFG_CRC16_MASK_));
+		lan743x_csr_write(adapter, MAC_WUF_MASK0(mask_index), 0x3000);
+		lan743x_csr_write(adapter, MAC_WUF_MASK1(mask_index), 0);
+		lan743x_csr_write(adapter, MAC_WUF_MASK2(mask_index), 0);
+		lan743x_csr_write(adapter, MAC_WUF_MASK3(mask_index), 0);
+		mask_index++;
+
+		wucsr |= MAC_WUCSR_RFE_WAKE_EN_ | MAC_WUCSR_WAKE_EN_;
+		macrx |= MAC_RX_RXEN_;
+		pmtctl |= PMT_CTL_WOL_EN_ | PMT_CTL_MAC_D3_RX_CLK_OVR_;
+		pmtctl |= PMT_CTL_RX_FCT_RFE_D3_CLK_OVR_;
+	}
+
+	lan743x_csr_write(adapter, MAC_WUCSR, wucsr);
+	lan743x_csr_write(adapter, PMT_CTL, pmtctl);
+	lan743x_csr_write(adapter, MAC_RX, macrx);
+}
+#endif /* CONFIG_PM */
+
+#if CONFIG_PM
+static int lan743x_pm_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct lan743x_adapter *adapter = netdev_priv(netdev);
+	u16 phydata;
+	int ret;
+
+	if (adapter->wolopts & WAKE_PHY) {
+		phydata = phy_read(netdev->phydev, 27);
+		phydata |= 0x0500;
+		phy_write(netdev->phydev, 27, phydata);
+	}
+
+	lan743x_pcidev_shutdown(pdev);
+
+	/* clear all wakes */
+	lan743x_csr_write(adapter, MAC_WUCSR, 0);
+	lan743x_csr_write(adapter, MAC_WUCSR2, 0);
+	lan743x_csr_write(adapter, MAC_WK_SRC, 0xFFFFFFFF);
+
+	if (adapter->wolopts)
+		lan743x_pm_set_wol(adapter);
+
+	/* Host sets PME_En, put D3hot */
+	ret = pci_prepare_to_sleep(pdev);
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+#if CONFIG_PM
+static int lan743x_pm_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct lan743x_adapter *adapter = netdev_priv(netdev);
+	int ret;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+	pci_save_state(pdev);
+
+	ret = lan743x_hardware_init(adapter, pdev);
+	if (ret) {
+		netif_err(adapter, probe, adapter->netdev,
+			  "lan743x_hardware_init returned %d\n", ret);
+	}
+
+	/* open netdev when netdev is at running state while resume.
+	 * For instance, it is true when system wakesup after pm-suspend
+	 * However, it is false when system wakes up after suspend GUI menu
+	 */
+	if (netif_running(netdev))
+		lan743x_netdev_open(netdev);
+
+	netif_device_attach(netdev);
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+#if CONFIG_PM
+const struct dev_pm_ops lan743x_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(lan743x_pm_suspend, lan743x_pm_resume)
+};
+#endif /*CONFIG_PM */
+
 static const struct pci_device_id lan743x_pcidev_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_SMSC, PCI_DEVICE_ID_SMSC_LAN7430) },
 	{ 0, }
@@ -2763,6 +2970,9 @@  static struct pci_driver lan743x_pcidev_driver = {
 	.id_table = lan743x_pcidev_tbl,
 	.probe    = lan743x_pcidev_probe,
 	.remove   = lan743x_pcidev_remove,
+#ifdef CONFIG_PM
+	.driver.pm = &lan743x_pm_ops,
+#endif
 	.shutdown = lan743x_pcidev_shutdown,
 };
 
diff --git a/drivers/net/ethernet/microchip/lan743x_main.h b/drivers/net/ethernet/microchip/lan743x_main.h
index c026b8d..72b9beb 100644
--- a/drivers/net/ethernet/microchip/lan743x_main.h
+++ b/drivers/net/ethernet/microchip/lan743x_main.h
@@ -24,8 +24,18 @@ 
 #define HW_CFG_LRST_				BIT(1)
 
 #define PMT_CTL					(0x014)
+#define PMT_CTL_ETH_PHY_D3_COLD_OVR_		BIT(27)
+#define PMT_CTL_MAC_D3_RX_CLK_OVR_		BIT(25)
+#define PMT_CTL_ETH_PHY_EDPD_PLL_CTL_		BIT(24)
+#define PMT_CTL_ETH_PHY_D3_OVR_			BIT(23)
+#define PMT_CTL_RX_FCT_RFE_D3_CLK_OVR_		BIT(18)
+#define PMT_CTL_GPIO_WAKEUP_EN_			BIT(15)
+#define PMT_CTL_EEE_WAKEUP_EN_			BIT(13)
 #define PMT_CTL_READY_				BIT(7)
 #define PMT_CTL_ETH_PHY_RST_			BIT(4)
+#define PMT_CTL_WOL_EN_				BIT(3)
+#define PMT_CTL_ETH_PHY_WAKE_EN_		BIT(2)
+#define PMT_CTL_WUPS_MASK_			(0x00000003)
 
 #define DP_SEL				(0x024)
 #define DP_SEL_DPRDY_			BIT(31)
@@ -107,6 +117,38 @@ 
 
 #define MAC_MII_DATA			(0x124)
 
+#define MAC_WUCSR				(0x140)
+#define MAC_WUCSR_RFE_WAKE_EN_			BIT(14)
+#define MAC_WUCSR_PFDA_EN_			BIT(3)
+#define MAC_WUCSR_WAKE_EN_			BIT(2)
+#define MAC_WUCSR_MPEN_				BIT(1)
+#define MAC_WUCSR_BCST_EN_			BIT(0)
+
+#define MAC_WK_SRC				(0x144)
+
+#define MAC_WUF_CFG0			(0x150)
+#define MAC_NUM_OF_WUF_CFG		(32)
+#define MAC_WUF_CFG_BEGIN		(MAC_WUF_CFG0)
+#define MAC_WUF_CFG(index)		(MAC_WUF_CFG_BEGIN + (4 * (index)))
+#define MAC_WUF_CFG_EN_			BIT(31)
+#define MAC_WUF_CFG_TYPE_MCAST_		(0x02000000)
+#define MAC_WUF_CFG_TYPE_ALL_		(0x01000000)
+#define MAC_WUF_CFG_OFFSET_SHIFT_	(16)
+#define MAC_WUF_CFG_CRC16_MASK_		(0x0000FFFF)
+
+#define MAC_WUF_MASK0_0			(0x200)
+#define MAC_WUF_MASK0_1			(0x204)
+#define MAC_WUF_MASK0_2			(0x208)
+#define MAC_WUF_MASK0_3			(0x20C)
+#define MAC_WUF_MASK0_BEGIN		(MAC_WUF_MASK0_0)
+#define MAC_WUF_MASK1_BEGIN		(MAC_WUF_MASK0_1)
+#define MAC_WUF_MASK2_BEGIN		(MAC_WUF_MASK0_2)
+#define MAC_WUF_MASK3_BEGIN		(MAC_WUF_MASK0_3)
+#define MAC_WUF_MASK0(index)		(MAC_WUF_MASK0_BEGIN + (0x10 * (index)))
+#define MAC_WUF_MASK1(index)		(MAC_WUF_MASK1_BEGIN + (0x10 * (index)))
+#define MAC_WUF_MASK2(index)		(MAC_WUF_MASK2_BEGIN + (0x10 * (index)))
+#define MAC_WUF_MASK3(index)		(MAC_WUF_MASK3_BEGIN + (0x10 * (index)))
+
 /* offset 0x400 - 0x500, x may range from 0 to 32, for a total of 33 entries */
 #define RFE_ADDR_FILT_HI(x)		(0x400 + (8 * (x)))
 #define RFE_ADDR_FILT_HI_VALID_		BIT(31)
@@ -121,6 +163,8 @@ 
 #define RFE_CTL_MCAST_HASH_		BIT(3)
 #define RFE_CTL_DA_PERFECT_		BIT(1)
 
+#define MAC_WUCSR2			(0x600)
+
 #define INT_STS				(0x780)
 #define INT_BIT_DMA_RX_(channel)	BIT(24 + (channel))
 #define INT_BIT_ALL_RX_			(0x0F000000)
@@ -534,6 +578,9 @@  struct lan743x_adapter {
 	struct net_device       *netdev;
 	struct mii_bus		*mdiobus;
 	int                     msg_enable;
+#ifdef CONFIG_PM
+	u32			wolopts;
+#endif
 	struct pci_dev		*pdev;
 	struct lan743x_csr      csr;
 	struct lan743x_intr     intr;