net: stmmac: RX queue routing configuration

[linux-2.6-block.git] / drivers / net / ethernet / stmicro / stmmac / dwmac4_core.c

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
index 342f62abb9cad5346a92698aa7ea7d9203106869..40ce202184025b5e393eccf60cd9aa06dfd4bcbd 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
@@ -109,6 +109,39 @@ static void dwmac4_tx_queue_priority(struct mac_device_info *hw,
        writel(value, ioaddr + base_register);
 }
 
+static void dwmac4_tx_queue_routing(struct mac_device_info *hw,
+                                   u8 packet, u32 queue)
+{
+       void __iomem *ioaddr = hw->pcsr;
+       u32 value;
+
+       const struct stmmac_rx_routing route_possibilities[] = {
+               { GMAC_RXQCTRL_AVCPQ_MASK, GMAC_RXQCTRL_AVCPQ_SHIFT },
+               { GMAC_RXQCTRL_PTPQ_MASK, GMAC_RXQCTRL_PTPQ_SHIFT },
+               { GMAC_RXQCTRL_DCBCPQ_MASK, GMAC_RXQCTRL_DCBCPQ_SHIFT },
+               { GMAC_RXQCTRL_UPQ_MASK, GMAC_RXQCTRL_UPQ_SHIFT },
+               { GMAC_RXQCTRL_MCBCQ_MASK, GMAC_RXQCTRL_MCBCQ_SHIFT },
+       };
+
+       value = readl(ioaddr + GMAC_RXQ_CTRL1);
+
+       /* routing configuration */
+       value &= ~route_possibilities[packet - 1].reg_mask;
+       value |= (queue << route_possibilities[packet-1].reg_shift) &
+                route_possibilities[packet - 1].reg_mask;
+
+       /* some packets require extra ops */
+       if (packet == PACKET_AVCPQ) {
+               value &= ~GMAC_RXQCTRL_TACPQE;
+               value |= 0x1 << GMAC_RXQCTRL_TACPQE_SHIFT;
+       } else if (packet == PACKET_MCBCQ) {
+               value &= ~GMAC_RXQCTRL_MCBCQEN;
+               value |= 0x1 << GMAC_RXQCTRL_MCBCQEN_SHIFT;
+       }
+
+       writel(value, ioaddr + GMAC_RXQ_CTRL1);
+}
+
 static void dwmac4_prog_mtl_rx_algorithms(struct mac_device_info *hw,
                                          u32 rx_alg)
 {
@@ -640,6 +673,7 @@ static const struct stmmac_ops dwmac4_ops = {
        .rx_queue_enable = dwmac4_rx_queue_enable,
        .rx_queue_prio = dwmac4_rx_queue_priority,
        .tx_queue_prio = dwmac4_tx_queue_priority,
+       .rx_queue_routing = dwmac4_tx_queue_routing,
        .prog_mtl_rx_algorithms = dwmac4_prog_mtl_rx_algorithms,
        .prog_mtl_tx_algorithms = dwmac4_prog_mtl_tx_algorithms,
        .set_mtl_tx_queue_weight = dwmac4_set_mtl_tx_queue_weight,