Commit a2f07512 authored by Justin Chen's avatar Justin Chen Committed by David S. Miller
Browse files

net: bcmasp: Add support for WoL magic packet 



Add support for Wake-On-Lan magic packet and magic packet with password.

Signed-off-by: default avatarJustin Chen <justin.chen@broadcom.com>
Signed-off-by: default avatarDavid S. Miller <davem@davemloft.net>
parent 490cb412

drivers/net/ethernet/broadcom/asp2/bcmasp.c

+144 −0
Original line number Diff line number Diff line
@@ -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;

drivers/net/ethernet/broadcom/asp2/bcmasp.h

+18 −0
Original line number Diff line number Diff line
@@ -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;

drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c

+36 −0
Original line number Diff line number Diff line
@@ -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,

};

drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c

+64 −12
Original line number Diff line number Diff line
@@ -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,6 +1324,7 @@ int bcmasp_interface_suspend(struct bcmasp_intf *intf) 


	bcmasp_netif_deinit(dev);



	if (!intf->wolopts) {

		ret = phy_suspend(dev->phydev);

		if (ret)

			goto out;
@@ -1306,6 +1338,10 @@ int bcmasp_interface_suspend(struct bcmasp_intf *intf) 
		 * 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 @@ 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;
@@ -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;