4 files changed, 36 insertions, 0 deletions

diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h b/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h
index 6203c7d8550f..45019649bbbd 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h
@@ -358,6 +358,8 @@ struct sxgbe_core_ops {
 	/* Enable disable checksum offload operations */
 	void (*enable_rx_csum)(void __iomem *ioaddr);
 	void (*disable_rx_csum)(void __iomem *ioaddr);
+	void (*enable_rxqueue)(void __iomem *ioaddr, int queue_num);
+	void (*disable_rxqueue)(void __iomem *ioaddr, int queue_num);
 };
 
 const struct sxgbe_core_ops *sxgbe_get_core_ops(void);
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c
index c4da7a2b002a..58c35692560e 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c
@@ -165,6 +165,26 @@ static void sxgbe_core_set_speed(void __iomem *ioaddr, unsigned char speed)
 	writel(tx_cfg, ioaddr + SXGBE_CORE_TX_CONFIG_REG);
 }
 
+static void sxgbe_core_enable_rxqueue(void __iomem *ioaddr, int queue_num)
+{
+	u32 reg_val;
+
+	reg_val = readl(ioaddr + SXGBE_CORE_RX_CTL0_REG);
+	reg_val &= ~(SXGBE_CORE_RXQ_ENABLE_MASK << queue_num);
+	reg_val |= SXGBE_CORE_RXQ_ENABLE;
+	writel(reg_val, ioaddr + SXGBE_CORE_RX_CTL0_REG);
+}
+
+static void sxgbe_core_disable_rxqueue(void __iomem *ioaddr, int queue_num)
+{
+	u32 reg_val;
+
+	reg_val = readl(ioaddr + SXGBE_CORE_RX_CTL0_REG);
+	reg_val &= ~(SXGBE_CORE_RXQ_ENABLE_MASK << queue_num);
+	reg_val |= SXGBE_CORE_RXQ_DISABLE;
+	writel(reg_val, ioaddr + SXGBE_CORE_RX_CTL0_REG);
+}
+
 static void  sxgbe_set_eee_mode(void __iomem *ioaddr)
 {
 	u32 ctrl;
@@ -254,6 +274,8 @@ static const struct sxgbe_core_ops core_ops = {
 	.set_eee_pls		= sxgbe_set_eee_pls,
 	.enable_rx_csum		= sxgbe_enable_rx_csum,
 	.disable_rx_csum	= sxgbe_disable_rx_csum,
+	.enable_rxqueue		= sxgbe_core_enable_rxqueue,
+	.disable_rxqueue	= sxgbe_core_disable_rxqueue,
 };
 
 const struct sxgbe_core_ops *sxgbe_get_core_ops(void)
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
index 6ad7b3aaaada..fd5c428411eb 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
@@ -1076,6 +1076,9 @@ static int sxgbe_open(struct net_device *dev)
 
 	/* Initialize the MAC Core */
 	priv->hw->mac->core_init(priv->ioaddr);
+	SXGBE_FOR_EACH_QUEUE(SXGBE_RX_QUEUES, queue_num) {
+		priv->hw->mac->enable_rxqueue(priv->ioaddr, queue_num);
+	}
 
 	/* Request the IRQ lines */
 	ret = devm_request_irq(priv->device, priv->irq, sxgbe_common_interrupt,
@@ -2240,9 +2243,14 @@ error_free_netdev:
 int sxgbe_drv_remove(struct net_device *ndev)
 {
 	struct sxgbe_priv_data *priv = netdev_priv(ndev);
+	u8 queue_num;
 
 	netdev_info(ndev, "%s: removing driver\n", __func__);
 
+	SXGBE_FOR_EACH_QUEUE(SXGBE_RX_QUEUES, queue_num) {
+		priv->hw->mac->disable_rxqueue(priv->ioaddr, queue_num);
+	}
+
 	priv->hw->dma->stop_rx(priv->ioaddr, SXGBE_RX_QUEUES);
 	priv->hw->dma->stop_tx(priv->ioaddr, SXGBE_TX_QUEUES);
 
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h b/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h
index 5a89acb4c505..56f8bf5a3f1b 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h
@@ -52,6 +52,10 @@
 #define SXGBE_CORE_RX_CTL2_REG		0x00A8
 #define SXGBE_CORE_RX_CTL3_REG		0x00AC
 
+#define SXGBE_CORE_RXQ_ENABLE_MASK	0x0003
+#define SXGBE_CORE_RXQ_ENABLE		0x0002
+#define SXGBE_CORE_RXQ_DISABLE		0x0000
+
 /* Interrupt Registers */
 #define SXGBE_CORE_INT_STATUS_REG	0x00B0
 #define SXGBE_CORE_INT_ENABLE_REG	0x00B4