net: bcmasp: Add support for WoL magic packet
Add support for Wake-On-Lan magic packet and magic packet with password. Signed-off-by: Justin Chen <justin.chen@broadcom.com> Signed-off-by: David S. Miller <davem@davemloft.net>
This commit is contained in:
Justin Chen
David S. Miller
parent
490cb41200
commit
a2f0751206
|
|
@ -436,6 +436,135 @@ void bcmasp_core_clock_set_intf(struct bcmasp_intf *intf, bool en)
|
|||
spin_unlock_irqrestore(&priv->clk_lock, flags);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
list_for_each_entry(intf, &priv->intfs, list) {
|
||||
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;
|
||||
|
||||
list_for_each_entry(intf, &priv->intfs, list) {
|
||||
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,
|
||||
|
|
@ -445,6 +574,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,
|
||||
};
|
||||
|
||||
|
|
@ -458,6 +590,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,
|
||||
};
|
||||
|
||||
|
|
@ -521,12 +656,16 @@ 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);
|
||||
INIT_LIST_HEAD(&priv->intfs);
|
||||
|
||||
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 */
|
||||
|
|
@ -570,6 +709,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.
|
||||
*/
|
||||
|
|
@ -585,6 +727,7 @@ static int bcmasp_probe(struct platform_device *pdev)
|
|||
if (ret) {
|
||||
netdev_err(intf->ndev,
|
||||
"failed to register net_device: %d\n", ret);
|
||||
priv->destroy_wol(priv);
|
||||
bcmasp_remove_intfs(priv);
|
||||
goto of_put_exit;
|
||||
}
|
||||
|
|
@ -605,6 +748,7 @@ static int bcmasp_remove(struct platform_device *pdev)
|
|||
if (!priv)
|
||||
return 0;
|
||||
|
||||
priv->destroy_wol(priv);
|
||||
bcmasp_remove_intfs(priv);
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -301,6 +301,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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1034,7 +1034,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;
|
||||
|
|
@ -1281,8 +1281,39 @@ void bcmasp_interface_destroy(struct bcmasp_intf *intf)
|
|||
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;
|
||||
|
||||
|
|
@ -1293,19 +1324,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);
|
||||
|
||||
|
|
@ -1316,6 +1352,20 @@ out:
|
|||
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;
|
||||
|
|
@ -1332,6 +1382,8 @@ int bcmasp_interface_resume(struct bcmasp_intf *intf)
|
|||
if (ret)
|
||||
goto out;
|
||||
|
||||
bcmasp_resume_from_wol(intf);
|
||||
|
||||
netif_device_attach(dev);
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue