Add support for Wake-On-Lan magic packet and magic packet with password.
Signed-off-by: Justin Chen <justin.chen@xxxxxxxxxxxx>
---
drivers/net/ethernet/broadcom/asp2/bcmasp.c | 148 +++++++++++++++++++++
drivers/net/ethernet/broadcom/asp2/bcmasp.h | 18 +++
.../net/ethernet/broadcom/asp2/bcmasp_ethtool.c | 36 +++++
drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c | 76 +++++++++--
4 files changed, 266 insertions(+), 12 deletions(-)
diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp.c b/drivers/net/ethernet/broadcom/asp2/bcmasp.c
index 34a0c7545ebd..40143bb566ef 100644
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp.c
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp.c
@@ -440,6 +440,139 @@ static int bcmasp_is_port_valid(struct bcmasp_priv *priv, int port)
return port == 0 || port == 1;
}
+static irqreturn_t bcmasp_isr_wol(int irq, void *data)
+{
+ struct bcmasp_priv *priv = data;
+ u32 status;
+
+ /* No L3 IRQ, so we good */
+ if (priv->wol_irq <= 0)
+ goto irq_handled;
+
+ status = wakeup_intr2_core_rl(priv, ASP_WAKEUP_INTR2_STATUS) &
+ ~wakeup_intr2_core_rl(priv, ASP_WAKEUP_INTR2_MASK_STATUS);
+ wakeup_intr2_core_wl(priv, status, ASP_WAKEUP_INTR2_CLEAR);
+
+irq_handled:
+ pm_wakeup_event(&priv->pdev->dev, 0);
+ return IRQ_HANDLED;
+}
+
+static int bcmasp_get_and_request_irq(struct bcmasp_priv *priv, int i)
+{
+ struct platform_device *pdev = priv->pdev;
+ int irq, ret;
+
+ irq = platform_get_irq_optional(pdev, i);
+ if (irq < 0)
+ return irq;
+
+ ret = devm_request_irq(&pdev->dev, irq, bcmasp_isr_wol, 0,
+ pdev->name, priv);
+ if (ret)
+ return ret;
+
+ return irq;
+}
+
+static void bcmasp_init_wol_shared(struct bcmasp_priv *priv)
+{
+ struct platform_device *pdev = priv->pdev;
+ struct device *dev = &pdev->dev;
+ int irq;
+
+ irq = bcmasp_get_and_request_irq(priv, 1);
+ if (irq < 0) {
+ dev_warn(dev, "Failed to init WoL irq: %d\n", irq);
+ return;
+ }
+
+ priv->wol_irq = irq;
+ priv->wol_irq_enabled_mask = 0;
+ device_set_wakeup_capable(&pdev->dev, 1);
+}
+
+static void bcmasp_enable_wol_shared(struct bcmasp_intf *intf, bool en)
+{
+ struct bcmasp_priv *priv = intf->parent;
+ struct device *dev = &priv->pdev->dev;
+
+ if (en) {
+ if (priv->wol_irq_enabled_mask) {
+ set_bit(intf->port, &priv->wol_irq_enabled_mask);
+ return;
+ }
+
+ /* First enable */
+ set_bit(intf->port, &priv->wol_irq_enabled_mask);
+ enable_irq_wake(priv->wol_irq);
+ device_set_wakeup_enable(dev, 1);
+ } else {
+ if (!priv->wol_irq_enabled_mask)
+ return;
+
+ clear_bit(intf->port, &priv->wol_irq_enabled_mask);
+ if (priv->wol_irq_enabled_mask)
+ return;
+
+ /* Last disable */
+ disable_irq_wake(priv->wol_irq);
+ device_set_wakeup_enable(dev, 0);
+ }
+}
+
+static void bcmasp_wol_irq_destroy_shared(struct bcmasp_priv *priv)
+{
+ if (priv->wol_irq > 0)
+ free_irq(priv->wol_irq, priv);
+}
+
+static void bcmasp_init_wol_per_intf(struct bcmasp_priv *priv)
+{
+ struct platform_device *pdev = priv->pdev;
+ struct device *dev = &pdev->dev;
+ struct bcmasp_intf *intf;
+ int irq, i;
+
+ for (i = 0; i < priv->intf_count; i++) {
+ intf = priv->intfs[i];
+ irq = bcmasp_get_and_request_irq(priv, intf->port + 1);
+ if (irq < 0) {
+ dev_warn(dev, "Failed to init WoL irq(port %d): %d\n",
+ intf->port, irq);
+ continue;
+ }
+
+ intf->wol_irq = irq;
+ intf->wol_irq_enabled = false;
+ device_set_wakeup_capable(&pdev->dev, 1);
+ }
+}
+
+static void bcmasp_enable_wol_per_intf(struct bcmasp_intf *intf, bool en)
+{
+ struct device *dev = &intf->parent->pdev->dev;
+
+ if (en ^ intf->wol_irq_enabled)
+ irq_set_irq_wake(intf->wol_irq, en);
+
+ intf->wol_irq_enabled = en;
+ device_set_wakeup_enable(dev, en);
+}
+
+static void bcmasp_wol_irq_destroy_per_intf(struct bcmasp_priv *priv)
+{
+ struct bcmasp_intf *intf;
+ int i;
+
+ for (i = 0; i < priv->intf_count; i++) {
+ intf = priv->intfs[i];
+
+ if (intf->wol_irq > 0)
+ free_irq(intf->wol_irq, priv);
+ }
+}
+
static struct bcmasp_hw_info v20_hw_info = {
.rx_ctrl_flush = ASP_RX_CTRL_FLUSH,
.umac2fb = UMAC2FB_OFFSET,
@@ -449,6 +582,9 @@ static struct bcmasp_hw_info v20_hw_info = {
};
static const struct bcmasp_plat_data v20_plat_data = {
+ .init_wol = bcmasp_init_wol_per_intf,
+ .enable_wol = bcmasp_enable_wol_per_intf,
+ .destroy_wol = bcmasp_wol_irq_destroy_per_intf,
.hw_info = &v20_hw_info,
};
@@ -462,6 +598,9 @@ static struct bcmasp_hw_info v21_hw_info = {
};
static const struct bcmasp_plat_data v21_plat_data = {
+ .init_wol = bcmasp_init_wol_shared,
+ .enable_wol = bcmasp_enable_wol_shared,
+ .destroy_wol = bcmasp_wol_irq_destroy_shared,
.hw_info = &v21_hw_info,
};
@@ -514,11 +653,15 @@ static int bcmasp_probe(struct platform_device *pdev)
priv->pdev = pdev;
spin_lock_init(&priv->mda_lock);
spin_lock_init(&priv->clk_lock);
+ mutex_init(&priv->wol_lock);
pdata = device_get_match_data(&pdev->dev);
if (!pdata)
return dev_err_probe(dev, -EINVAL, "unable to find platform data\n");
+ priv->init_wol = pdata->init_wol;
+ priv->enable_wol = pdata->enable_wol;
+ priv->destroy_wol = pdata->destroy_wol;
priv->hw_info = pdata->hw_info;
/* Enable all clocks to ensure successful probing */
@@ -578,6 +721,9 @@ static int bcmasp_probe(struct platform_device *pdev)
i++;
}
+ /* Check and enable WoL */
+ priv->init_wol(priv);
+
/* Drop the clock reference count now and let ndo_open()/ndo_close()
* manage it for us from now on.
*/
@@ -616,6 +762,8 @@ static int bcmasp_remove(struct platform_device *pdev)
struct bcmasp_intf *intf;
int i;
+ priv->destroy_wol(priv);
+
for (i = 0; i < priv->intf_count; i++) {
intf = priv->intfs[i];
if (!intf)
diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp.h b/drivers/net/ethernet/broadcom/asp2/bcmasp.h
index 0f5c285c6187..c8b66a6af584 100644
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp.h
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp.h
@@ -299,6 +299,12 @@ struct bcmasp_intf {
/* Statistics */
struct bcmasp_intf_stats64 stats64;
+
+ u32 wolopts;
+ u8 sopass[SOPASS_MAX];
+ /* Used if per intf wol irq */
+ int wol_irq;
+ unsigned int wol_irq_enabled:1;
};
#define NUM_MDA_FILTERS 32
@@ -321,6 +327,9 @@ struct bcmasp_hw_info {
};
struct bcmasp_plat_data {
+ void (*init_wol)(struct bcmasp_priv *priv);
+ void (*enable_wol)(struct bcmasp_intf *intf, bool en);
+ void (*destroy_wol)(struct bcmasp_priv *priv);
struct bcmasp_hw_info *hw_info;
};
@@ -331,6 +340,15 @@ struct bcmasp_priv {
int irq;
u32 irq_mask;
+ /* Used if shared wol irq */
+ struct mutex wol_lock;
+ int wol_irq;
+ unsigned long wol_irq_enabled_mask;
+
+ void (*init_wol)(struct bcmasp_priv *priv);
+ void (*enable_wol)(struct bcmasp_intf *intf, bool en);
+ void (*destroy_wol)(struct bcmasp_priv *priv);
+
void __iomem *base;
struct bcmasp_hw_info *hw_info;
diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c b/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c
index 394c0e1cb026..ae24a1f74d49 100644
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c
@@ -30,6 +30,40 @@ static void bcmasp_set_msglevel(struct net_device *dev, u32 level)
intf->msg_enable = level;
}
+#define BCMASP_SUPPORTED_WAKE (WAKE_MAGIC | WAKE_MAGICSECURE)
+static void bcmasp_get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
+{
+ struct bcmasp_intf *intf = netdev_priv(dev);
+
+ wol->supported = BCMASP_SUPPORTED_WAKE;
+ wol->wolopts = intf->wolopts;
+ memset(wol->sopass, 0, sizeof(wol->sopass));
+
+ if (wol->wolopts & WAKE_MAGICSECURE)
+ memcpy(wol->sopass, intf->sopass, sizeof(intf->sopass));
+}
+
+static int bcmasp_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
+{
+ struct bcmasp_intf *intf = netdev_priv(dev);
+ struct bcmasp_priv *priv = intf->parent;
+ struct device *kdev = &priv->pdev->dev;
+
+ if (!device_can_wakeup(kdev))
+ return -EOPNOTSUPP;
+
+ /* Interface Specific */
+ intf->wolopts = wol->wolopts;
+ if (intf->wolopts & WAKE_MAGICSECURE)
+ memcpy(intf->sopass, wol->sopass, sizeof(wol->sopass));
+
+ mutex_lock(&priv->wol_lock);
+ priv->enable_wol(intf, !!intf->wolopts);
+ mutex_unlock(&priv->wol_lock);
+
+ return 0;
+}
+
const struct ethtool_ops bcmasp_ethtool_ops = {
.get_drvinfo = bcmasp_get_drvinfo,
.get_link = ethtool_op_get_link,
@@ -37,4 +71,6 @@ const struct ethtool_ops bcmasp_ethtool_ops = {
.set_link_ksettings = phy_ethtool_set_link_ksettings,
.get_msglevel = bcmasp_get_msglevel,
.set_msglevel = bcmasp_set_msglevel,
+ .get_wol = bcmasp_get_wol,
+ .set_wol = bcmasp_set_wol,
};
diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c b/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c
index 3882d65541c0..1f6f050f9059 100644
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c
@@ -1041,7 +1041,7 @@ static int bcmasp_netif_init(struct net_device *dev, bool phy_connect)
netdev_err(dev, "could not attach to PHY\n");
goto err_phy_disable;
}
- } else {
+ } else if (!intf->wolopts) {
ret = phy_resume(dev->phydev);
if (ret)
goto err_phy_disable;
@@ -1297,8 +1297,39 @@ void bcmasp_interface_destroy(struct bcmasp_intf *intf, bool unregister)
free_netdev(intf->ndev);
}
+static void bcmasp_suspend_to_wol(struct bcmasp_intf *intf)
+{
+ struct net_device *ndev = intf->ndev;
+ u32 reg;
+
+ reg = umac_rl(intf, UMC_MPD_CTRL);
+ if (intf->wolopts & (WAKE_MAGIC | WAKE_MAGICSECURE))
+ reg |= UMC_MPD_CTRL_MPD_EN;
+ reg &= ~UMC_MPD_CTRL_PSW_EN;
+ if (intf->wolopts & WAKE_MAGICSECURE) {
+ /* Program the SecureOn password */
+ umac_wl(intf, get_unaligned_be16(&intf->sopass[0]),
+ UMC_PSW_MS);
+ umac_wl(intf, get_unaligned_be32(&intf->sopass[2]),
+ UMC_PSW_LS);
+ reg |= UMC_MPD_CTRL_PSW_EN;
+ }
+ umac_wl(intf, reg, UMC_MPD_CTRL);
+
+ /* UniMAC receive needs to be turned on */
+ umac_enable_set(intf, UMC_CMD_RX_EN, 1);
+
+ if (intf->parent->wol_irq > 0) {
+ wakeup_intr2_core_wl(intf->parent, 0xffffffff,
+ ASP_WAKEUP_INTR2_MASK_CLEAR);
+ }
+
+ netif_dbg(intf, wol, ndev, "entered WOL mode\n");
+}
+
int bcmasp_interface_suspend(struct bcmasp_intf *intf)
{
+ struct device *kdev = &intf->parent->pdev->dev;
struct net_device *dev = intf->ndev;
int ret = 0;
@@ -1309,19 +1340,24 @@ int bcmasp_interface_suspend(struct bcmasp_intf *intf)
bcmasp_netif_deinit(dev);
- ret = phy_suspend(dev->phydev);
- if (ret)
- goto out;
+ if (!intf->wolopts) {
+ ret = phy_suspend(dev->phydev);
+ if (ret)
+ goto out;
- if (intf->internal_phy)
- bcmasp_ephy_enable_set(intf, false);
- else
- bcmasp_rgmii_mode_en_set(intf, false);
+ if (intf->internal_phy)
+ bcmasp_ephy_enable_set(intf, false);
+ else
+ bcmasp_rgmii_mode_en_set(intf, false);
- /* If Wake-on-LAN is disabled, we can safely
- * disable the network interface clocks.
- */
- bcmasp_core_clock_set_intf(intf, false);
+ /* If Wake-on-LAN is disabled, we can safely
+ * disable the network interface clocks.
+ */
+ bcmasp_core_clock_set_intf(intf, false);
+ }
+
+ if (device_may_wakeup(kdev) && intf->wolopts)
+ bcmasp_suspend_to_wol(intf);
clk_disable_unprepare(intf->parent->clk);
@@ -1332,6 +1368,20 @@ int bcmasp_interface_suspend(struct bcmasp_intf *intf)
return ret;
}
+static void bcmasp_resume_from_wol(struct bcmasp_intf *intf)
+{
+ u32 reg;
+
+ reg = umac_rl(intf, UMC_MPD_CTRL);
+ reg &= ~UMC_MPD_CTRL_MPD_EN;
+ umac_wl(intf, reg, UMC_MPD_CTRL);
+
+ if (intf->parent->wol_irq > 0) {
+ wakeup_intr2_core_wl(intf->parent, 0xffffffff,
+ ASP_WAKEUP_INTR2_MASK_SET);
+ }
+}
+
int bcmasp_interface_resume(struct bcmasp_intf *intf)
{
struct net_device *dev = intf->ndev;
@@ -1348,6 +1398,8 @@ int bcmasp_interface_resume(struct bcmasp_intf *intf)
if (ret)
goto out;
+ bcmasp_resume_from_wol(intf);
+
netif_device_attach(dev);
return 0;
--
2.7.4
Attachment:
smime.p7s
Description: S/MIME Cryptographic Signature
