5 files changed, 74 insertions, 58 deletions

diff --git a/net/core/dev.c b/net/core/dev.c
index 69378f25069..f027a1ac4fb 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -1598,7 +1598,8 @@ static int dev_gso_segment(struct sk_buff *skb)
         return 0;
 }
 
-int dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+int dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev,
+                        struct netdev_queue *txq)
 {
         if (likely(!skb->next)) {
                 if (!list_empty(&ptype_all))
@@ -1627,9 +1628,7 @@ gso:
                         skb->next = nskb;
                         return rc;
                 }
-                if (unlikely((netif_queue_stopped(dev) ||
-                             netif_subqueue_stopped(dev, skb)) &&
-                             skb->next))
+                if (unlikely(netif_tx_queue_stopped(txq) && skb->next))
                         return NETDEV_TX_BUSY;
         } while (skb->next);
 
@@ -1669,7 +1668,10 @@ out_kfree_skb:
 static struct netdev_queue *dev_pick_tx(struct net_device *dev,
                                         struct sk_buff *skb)
 {
-        return netdev_get_tx_queue(dev, 0);
+        u16 queue_index = 0;
+
+        skb_set_queue_mapping(skb, queue_index);
+        return netdev_get_tx_queue(dev, queue_index);
 }
 
 int dev_queue_xmit(struct sk_buff *skb)
@@ -1737,8 +1739,6 @@ gso:
                 spin_lock(&txq->lock);
                 q = txq->qdisc;
                 if (q->enqueue) {
-                        /* reset queue_mapping to zero */
-                        skb_set_queue_mapping(skb, 0);
                         rc = q->enqueue(skb, q);
                         qdisc_run(txq);
                         spin_unlock(&txq->lock);
@@ -1768,10 +1768,9 @@ gso:
 
                         HARD_TX_LOCK(dev, txq, cpu);
 
-                        if (!netif_queue_stopped(dev) &&
-                            !netif_subqueue_stopped(dev, skb)) {
+                        if (!netif_tx_queue_stopped(txq)) {
                                 rc = 0;
-                                if (!dev_hard_start_xmit(skb, dev)) {
+                                if (!dev_hard_start_xmit(skb, dev, txq)) {
                                         HARD_TX_UNLOCK(dev, txq);
                                         goto out;
                                 }
@@ -4160,8 +4159,7 @@ struct net_device *alloc_netdev_mq(int sizeof_priv, const char *name,
 
         BUG_ON(strlen(name) >= sizeof(dev->name));
 
-        alloc_size = sizeof(struct net_device) +
-                     sizeof(struct net_device_subqueue) * (queue_count - 1);
+        alloc_size = sizeof(struct net_device);
         if (sizeof_priv) {
                 /* ensure 32-byte alignment of private area */
                 alloc_size = (alloc_size + NETDEV_ALIGN_CONST) & ~NETDEV_ALIGN_CONST;
@@ -4191,16 +4189,14 @@ struct net_device *alloc_netdev_mq(int sizeof_priv, const char *name,
 
         dev->_tx = tx;
         dev->num_tx_queues = queue_count;
+        dev->real_num_tx_queues = queue_count;
 
         if (sizeof_priv) {
                 dev->priv = ((char *)dev +
-                             ((sizeof(struct net_device) +
-                               (sizeof(struct net_device_subqueue) *
-                                (queue_count - 1)) + NETDEV_ALIGN_CONST)
+                             ((sizeof(struct net_device) + NETDEV_ALIGN_CONST)
                               & ~NETDEV_ALIGN_CONST));
         }
 
-        dev->egress_subqueue_count = queue_count;
         dev->gso_max_size = GSO_MAX_SIZE;
 
         netdev_init_queues(dev);
diff --git a/net/core/netpoll.c b/net/core/netpoll.c
index 8fb134da034..c12720895ec 100644
--- a/net/core/netpoll.c
+++ b/net/core/netpoll.c
@@ -58,25 +58,27 @@ static void queue_process(struct work_struct *work)
 
         while ((skb = skb_dequeue(&npinfo->txq))) {
                 struct net_device *dev = skb->dev;
+                struct netdev_queue *txq;
 
                 if (!netif_device_present(dev) || !netif_running(dev)) {
                         __kfree_skb(skb);
                         continue;
                 }
 
+                txq = netdev_get_tx_queue(dev, skb_get_queue_mapping(skb));
+
                 local_irq_save(flags);
-                netif_tx_lock(dev);
-                if ((netif_queue_stopped(dev) ||
-                     netif_subqueue_stopped(dev, skb)) ||
-                     dev->hard_start_xmit(skb, dev) != NETDEV_TX_OK) {
+                __netif_tx_lock(txq, smp_processor_id());
+                if (netif_tx_queue_stopped(txq) ||
+                    dev->hard_start_xmit(skb, dev) != NETDEV_TX_OK) {
                         skb_queue_head(&npinfo->txq, skb);
-                        netif_tx_unlock(dev);
+                        __netif_tx_unlock(txq);
                         local_irq_restore(flags);
 
                         schedule_delayed_work(&npinfo->tx_work, HZ/10);
                         return;
                 }
-                netif_tx_unlock(dev);
+                __netif_tx_unlock(txq);
                 local_irq_restore(flags);
         }
 }
@@ -278,17 +280,19 @@ static void netpoll_send_skb(struct netpoll *np, struct sk_buff *skb)
 
         /* don't get messages out of order, and no recursion */
         if (skb_queue_len(&npinfo->txq) == 0 && !netpoll_owner_active(dev)) {
+                struct netdev_queue *txq;
                 unsigned long flags;
 
+                txq = netdev_get_tx_queue(dev, skb_get_queue_mapping(skb));
+
                 local_irq_save(flags);
                 /* try until next clock tick */
                 for (tries = jiffies_to_usecs(1)/USEC_PER_POLL;
                      tries > 0; --tries) {
-                        if (netif_tx_trylock(dev)) {
-                                if (!netif_queue_stopped(dev) &&
-                                    !netif_subqueue_stopped(dev, skb))
+                        if (__netif_tx_trylock(txq)) {
+                                if (!netif_tx_queue_stopped(txq))
                                         status = dev->hard_start_xmit(skb, dev);
-                                netif_tx_unlock(dev);
+                                __netif_tx_unlock(txq);
 
                                 if (status == NETDEV_TX_OK)
                                         break;
diff --git a/net/core/pktgen.c b/net/core/pktgen.c
index fdf537707e5..906802db4ed 100644
--- a/net/core/pktgen.c
+++ b/net/core/pktgen.c
@@ -2123,6 +2123,24 @@ static void get_ipsec_sa(struct pktgen_dev *pkt_dev, int flow)
         }
 }
 #endif
+static void set_cur_queue_map(struct pktgen_dev *pkt_dev)
+{
+        if (pkt_dev->queue_map_min < pkt_dev->queue_map_max) {
+                __u16 t;
+                if (pkt_dev->flags & F_QUEUE_MAP_RND) {
+                        t = random32() %
+                                (pkt_dev->queue_map_max -
+                                 pkt_dev->queue_map_min + 1)
+                                + pkt_dev->queue_map_min;
+                } else {
+                        t = pkt_dev->cur_queue_map + 1;
+                        if (t > pkt_dev->queue_map_max)
+                                t = pkt_dev->queue_map_min;
+                }
+                pkt_dev->cur_queue_map = t;
+        }
+}
+
 /* Increment/randomize headers according to flags and current values
  * for IP src/dest, UDP src/dst port, MAC-Addr src/dst
  */
@@ -2325,19 +2343,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
                 pkt_dev->cur_pkt_size = t;
         }
 
-        if (pkt_dev->queue_map_min < pkt_dev->queue_map_max) {
-                __u16 t;
-                if (pkt_dev->flags & F_QUEUE_MAP_RND) {
-                        t = random32() %
-                                (pkt_dev->queue_map_max - pkt_dev->queue_map_min + 1)
-                                + pkt_dev->queue_map_min;
-                } else {
-                        t = pkt_dev->cur_queue_map + 1;
-                        if (t > pkt_dev->queue_map_max)
-                                t = pkt_dev->queue_map_min;
-                }
-                pkt_dev->cur_queue_map = t;
-        }
+        set_cur_queue_map(pkt_dev);
 
         pkt_dev->flows[flow].count++;
 }
@@ -2458,7 +2464,7 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
         __be16 *vlan_encapsulated_proto = NULL;  /* packet type ID field (or len) for VLAN tag */
         __be16 *svlan_tci = NULL;                /* Encapsulates priority and SVLAN ID */
         __be16 *svlan_encapsulated_proto = NULL; /* packet type ID field (or len) for SVLAN tag */
-
+        u16 queue_map;
 
         if (pkt_dev->nr_labels)
                 protocol = htons(ETH_P_MPLS_UC);
@@ -2469,6 +2475,7 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
         /* Update any of the values, used when we're incrementing various
          * fields.
          */
+        queue_map = pkt_dev->cur_queue_map;
         mod_cur_headers(pkt_dev);
 
         datalen = (odev->hard_header_len + 16) & ~0xf;
@@ -2507,7 +2514,7 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
         skb->network_header = skb->tail;
         skb->transport_header = skb->network_header + sizeof(struct iphdr);
         skb_put(skb, sizeof(struct iphdr) + sizeof(struct udphdr));
-        skb_set_queue_mapping(skb, pkt_dev->cur_queue_map);
+        skb_set_queue_mapping(skb, queue_map);
         iph = ip_hdr(skb);
         udph = udp_hdr(skb);
 
@@ -2797,6 +2804,7 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
         __be16 *vlan_encapsulated_proto = NULL;  /* packet type ID field (or len) for VLAN tag */
         __be16 *svlan_tci = NULL;                /* Encapsulates priority and SVLAN ID */
         __be16 *svlan_encapsulated_proto = NULL; /* packet type ID field (or len) for SVLAN tag */
+        u16 queue_map;
 
         if (pkt_dev->nr_labels)
                 protocol = htons(ETH_P_MPLS_UC);
@@ -2807,6 +2815,7 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
         /* Update any of the values, used when we're incrementing various
          * fields.
          */
+        queue_map = pkt_dev->cur_queue_map;
         mod_cur_headers(pkt_dev);
 
         skb = alloc_skb(pkt_dev->cur_pkt_size + 64 + 16 +
@@ -2844,7 +2853,7 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
         skb->network_header = skb->tail;
         skb->transport_header = skb->network_header + sizeof(struct ipv6hdr);
         skb_put(skb, sizeof(struct ipv6hdr) + sizeof(struct udphdr));
-        skb_set_queue_mapping(skb, pkt_dev->cur_queue_map);
+        skb_set_queue_mapping(skb, queue_map);
         iph = ipv6_hdr(skb);
         udph = udp_hdr(skb);
 
@@ -3263,7 +3272,9 @@ static void pktgen_rem_thread(struct pktgen_thread *t)
 static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
 {
         struct net_device *odev = NULL;
+        struct netdev_queue *txq;
         __u64 idle_start = 0;
+        u16 queue_map;
         int ret;
 
         odev = pkt_dev->odev;
@@ -3285,9 +3296,15 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
                 }
         }
 
-        if ((netif_queue_stopped(odev) ||
-             (pkt_dev->skb &&
-              netif_subqueue_stopped(odev, pkt_dev->skb))) ||
+        if (!pkt_dev->skb) {
+                set_cur_queue_map(pkt_dev);
+                queue_map = pkt_dev->cur_queue_map;
+        } else {
+                queue_map = skb_get_queue_mapping(pkt_dev->skb);
+        }
+
+        txq = netdev_get_tx_queue(odev, queue_map);
+        if (netif_tx_queue_stopped(txq) ||
             need_resched()) {
                 idle_start = getCurUs();
 
@@ -3303,8 +3320,7 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
 
                 pkt_dev->idle_acc += getCurUs() - idle_start;
 
-                if (netif_queue_stopped(odev) ||
-                    netif_subqueue_stopped(odev, pkt_dev->skb)) {
+                if (netif_tx_queue_stopped(txq)) {
                         pkt_dev->next_tx_us = getCurUs();       /* TODO */
                         pkt_dev->next_tx_ns = 0;
                         goto out;       /* Try the next interface */
@@ -3331,9 +3347,12 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
                 }
         }
 
-        netif_tx_lock_bh(odev);
-        if (!netif_queue_stopped(odev) &&
-            !netif_subqueue_stopped(odev, pkt_dev->skb)) {
+        /* fill_packet() might have changed the queue */
+        queue_map = skb_get_queue_mapping(pkt_dev->skb);
+        txq = netdev_get_tx_queue(odev, queue_map);
+
+        __netif_tx_lock_bh(txq);
+        if (!netif_tx_queue_stopped(txq)) {
 
                 atomic_inc(&(pkt_dev->skb->users));
               retry_now:
@@ -3377,7 +3396,7 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
                 pkt_dev->next_tx_ns = 0;
         }
 
-        netif_tx_unlock_bh(odev);
+        __netif_tx_unlock_bh(txq);
 
         /* If pkt_dev->count is zero, then run forever */
         if ((pkt_dev->count != 0) && (pkt_dev->sofar >= pkt_dev->count)) {
diff --git a/net/sched/sch_generic.c b/net/sched/sch_generic.c
index 4e2b865cbba..2f575b9017d 100644
--- a/net/sched/sch_generic.c
+++ b/net/sched/sch_generic.c
@@ -166,7 +166,7 @@ static inline int qdisc_restart(struct netdev_queue *txq)
 
         HARD_TX_LOCK(dev, txq, smp_processor_id());
         if (!netif_subqueue_stopped(dev, skb))
-                ret = dev_hard_start_xmit(skb, dev);
+                ret = dev_hard_start_xmit(skb, dev, txq);
         HARD_TX_UNLOCK(dev, txq);
 
         spin_lock(&txq->lock);
@@ -198,11 +198,10 @@ static inline int qdisc_restart(struct netdev_queue *txq)
 
 void __qdisc_run(struct netdev_queue *txq)
 {
-        struct net_device *dev = txq->dev;
         unsigned long start_time = jiffies;
 
         while (qdisc_restart(txq)) {
-                if (netif_queue_stopped(dev))
+                if (netif_tx_queue_stopped(txq))
                         break;
 
                 /*
diff --git a/net/sched/sch_teql.c b/net/sched/sch_teql.c
index 44a2c3451f4..ade3372221c 100644
--- a/net/sched/sch_teql.c
+++ b/net/sched/sch_teql.c
@@ -295,8 +295,7 @@ restart:
                 slave_txq = netdev_get_tx_queue(slave, 0);
                 if (slave_txq->qdisc_sleeping != q)
                         continue;
-                if (netif_queue_stopped(slave) ||
-                    __netif_subqueue_stopped(slave, subq) ||
+                if (__netif_subqueue_stopped(slave, subq) ||
                     !netif_running(slave)) {
                         busy = 1;
                         continue;
@@ -305,8 +304,7 @@ restart:
                 switch (teql_resolve(skb, skb_res, slave)) {
                 case 0:
                         if (netif_tx_trylock(slave)) {
-                                if (!netif_queue_stopped(slave) &&
-                                    !__netif_subqueue_stopped(slave, subq) &&
+                                if (!__netif_subqueue_stopped(slave, subq) &&
                                     slave->hard_start_xmit(skb, slave) == 0) {
                                         netif_tx_unlock(slave);
                                         master->slaves = NEXT_SLAVE(q);