net: sxgbe: Added rxqueue enable function

This patch adds rxqueue enable function according to number of rxqueue
and adds rxqueue disable function for removing.

Signed-off-by: Byungho An <bh74.an@samsung.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

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