6 files changed, 51 insertions, 21 deletions

diff --git a/net/core/dev.c b/net/core/dev.c
index 6dce9d2d46..7ddf66d0ad 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -1429,7 +1429,9 @@ gso:
                         skb->next = nskb;
                         return rc;
                 }
-                if (unlikely(netif_queue_stopped(dev) && skb->next))
+                if (unlikely((netif_queue_stopped(dev) ||
+                             netif_subqueue_stopped(dev, skb->queue_mapping)) &&
+                             skb->next))
                         return NETDEV_TX_BUSY;
         } while (skb->next);
 
@@ -1547,6 +1549,8 @@ gso:
                 spin_lock(&dev->queue_lock);
                 q = dev->qdisc;
                 if (q->enqueue) {
+                        /* reset queue_mapping to zero */
+                        skb->queue_mapping = 0;
                         rc = q->enqueue(skb, q);
                         qdisc_run(dev);
                         spin_unlock(&dev->queue_lock);
@@ -1576,7 +1580,8 @@ gso:
 
                         HARD_TX_LOCK(dev, cpu);
 
-                        if (!netif_queue_stopped(dev)) {
+                        if (!netif_queue_stopped(dev) &&
+                            !netif_subqueue_stopped(dev, skb->queue_mapping)) {
                                 rc = 0;
                                 if (!dev_hard_start_xmit(skb, dev)) {
                                         HARD_TX_UNLOCK(dev);
@@ -3539,16 +3544,18 @@ static struct net_device_stats *internal_stats(struct net_device *dev)
 }
 
 /**
- *      alloc_netdev - allocate network device
+ *      alloc_netdev_mq - allocate network device
  *      @sizeof_priv:   size of private data to allocate space for
  *      @name:          device name format string
  *      @setup:         callback to initialize device
+ *      @queue_count:   the number of subqueues to allocate
  *
  *      Allocates a struct net_device with private data area for driver use
- *      and performs basic initialization.
+ *      and performs basic initialization.  Also allocates subquue structs
+ *      for each queue on the device at the end of the netdevice.
  */
-struct net_device *alloc_netdev(int sizeof_priv, const char *name,
-                void (*setup)(struct net_device *))
+struct net_device *alloc_netdev_mq(int sizeof_priv, const char *name,
+                void (*setup)(struct net_device *), unsigned int queue_count)
 {
         void *p;
         struct net_device *dev;
@@ -3557,7 +3564,9 @@ struct net_device *alloc_netdev(int sizeof_priv, const char *name,
         BUG_ON(strlen(name) >= sizeof(dev->name));
 
         /* ensure 32-byte alignment of both the device and private area */
-        alloc_size = (sizeof(*dev) + NETDEV_ALIGN_CONST) & ~NETDEV_ALIGN_CONST;
+        alloc_size = (sizeof(*dev) + NETDEV_ALIGN_CONST +
+                     (sizeof(struct net_device_subqueue) * queue_count)) &
+                     ~NETDEV_ALIGN_CONST;
         alloc_size += sizeof_priv + NETDEV_ALIGN_CONST;
 
         p = kzalloc(alloc_size, GFP_KERNEL);
@@ -3570,15 +3579,22 @@ struct net_device *alloc_netdev(int sizeof_priv, const char *name,
                 (((long)p + NETDEV_ALIGN_CONST) & ~NETDEV_ALIGN_CONST);
         dev->padded = (char *)dev - (char *)p;
 
-        if (sizeof_priv)
-                dev->priv = netdev_priv(dev);
+        if (sizeof_priv) {
+                dev->priv = ((char *)dev +
+                             ((sizeof(struct net_device) +
+                               (sizeof(struct net_device_subqueue) *
+                                queue_count) + NETDEV_ALIGN_CONST)
+                              & ~NETDEV_ALIGN_CONST));
+        }
+
+        dev->egress_subqueue_count = queue_count;
 
         dev->get_stats = internal_stats;
         setup(dev);
         strcpy(dev->name, name);
         return dev;
 }
-EXPORT_SYMBOL(alloc_netdev);
+EXPORT_SYMBOL(alloc_netdev_mq);
 
 /**
  *      free_netdev - free network device
diff --git a/net/core/netpoll.c b/net/core/netpoll.c
index a0efdd7a6b..4b06d19363 100644
--- a/net/core/netpoll.c
+++ b/net/core/netpoll.c
@@ -66,8 +66,9 @@ static void queue_process(struct work_struct *work)
 
                 local_irq_save(flags);
                 netif_tx_lock(dev);
-                if (netif_queue_stopped(dev) ||
-                    dev->hard_start_xmit(skb, dev) != NETDEV_TX_OK) {
+                if ((netif_queue_stopped(dev) ||
+                     netif_subqueue_stopped(dev, skb->queue_mapping)) ||
+                     dev->hard_start_xmit(skb, dev) != NETDEV_TX_OK) {
                         skb_queue_head(&npinfo->txq, skb);
                         netif_tx_unlock(dev);
                         local_irq_restore(flags);
@@ -254,7 +255,8 @@ static void netpoll_send_skb(struct netpoll *np, struct sk_buff *skb)
                 for (tries = jiffies_to_usecs(1)/USEC_PER_POLL;
                      tries > 0; --tries) {
                         if (netif_tx_trylock(dev)) {
-                                if (!netif_queue_stopped(dev))
+                                if (!netif_queue_stopped(dev) &&
+                                    !netif_subqueue_stopped(dev, skb->queue_mapping))
                                         status = dev->hard_start_xmit(skb, dev);
                                 netif_tx_unlock(dev);
 
diff --git a/net/core/pktgen.c b/net/core/pktgen.c
index 9cd3a1cb60..dffe067e7a 100644
--- a/net/core/pktgen.c
+++ b/net/core/pktgen.c
@@ -3139,7 +3139,9 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
                 }
         }
 
-        if (netif_queue_stopped(odev) || need_resched()) {
+        if ((netif_queue_stopped(odev) ||
+             netif_subqueue_stopped(odev, pkt_dev->skb->queue_mapping)) ||
+             need_resched()) {
                 idle_start = getCurUs();
 
                 if (!netif_running(odev)) {
@@ -3154,7 +3156,8 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
 
                 pkt_dev->idle_acc += getCurUs() - idle_start;
 
-                if (netif_queue_stopped(odev)) {
+                if (netif_queue_stopped(odev) ||
+                    netif_subqueue_stopped(odev, pkt_dev->skb->queue_mapping)) {
                         pkt_dev->next_tx_us = getCurUs();       /* TODO */
                         pkt_dev->next_tx_ns = 0;
                         goto out;       /* Try the next interface */
@@ -3181,7 +3184,8 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
         }
 
         netif_tx_lock_bh(odev);
-        if (!netif_queue_stopped(odev)) {
+        if (!netif_queue_stopped(odev) &&
+            !netif_subqueue_stopped(odev, pkt_dev->skb->queue_mapping)) {
 
                 atomic_inc(&(pkt_dev->skb->users));
               retry_now:
diff --git a/net/core/skbuff.c b/net/core/skbuff.c
index c989c3a0f9..6a41b96b3d 100644
--- a/net/core/skbuff.c
+++ b/net/core/skbuff.c
@@ -419,6 +419,7 @@ struct sk_buff *skb_clone(struct sk_buff *skb, gfp_t gfp_mask)
         n->nohdr = 0;
         C(pkt_type);
         C(ip_summed);
+        skb_copy_queue_mapping(n, skb);
         C(priority);
 #if defined(CONFIG_IP_VS) || defined(CONFIG_IP_VS_MODULE)
         C(ipvs_property);
@@ -460,6 +461,7 @@ static void copy_skb_header(struct sk_buff *new, const struct sk_buff *old)
 #endif
         new->sk         = NULL;
         new->dev        = old->dev;
+        skb_copy_queue_mapping(new, old);
         new->priority   = old->priority;
         new->protocol   = old->protocol;
         new->dst        = dst_clone(old->dst);
@@ -1932,6 +1934,7 @@ struct sk_buff *skb_segment(struct sk_buff *skb, int features)
                 tail = nskb;
 
                 nskb->dev = skb->dev;
+                skb_copy_queue_mapping(nskb, skb);
                 nskb->priority = skb->priority;
                 nskb->protocol = skb->protocol;
                 nskb->dst = dst_clone(skb->dst);
diff --git a/net/ethernet/eth.c b/net/ethernet/eth.c
index 0ac2524f3b..1387e5411f 100644
--- a/net/ethernet/eth.c
+++ b/net/ethernet/eth.c
@@ -316,9 +316,10 @@ void ether_setup(struct net_device *dev)
 EXPORT_SYMBOL(ether_setup);
 
 /**
- * alloc_etherdev - Allocates and sets up an Ethernet device
+ * alloc_etherdev_mq - Allocates and sets up an Ethernet device
  * @sizeof_priv: Size of additional driver-private structure to be allocated
  *      for this Ethernet device
+ * @queue_count: The number of queues this device has.
  *
  * Fill in the fields of the device structure with Ethernet-generic
  * values. Basically does everything except registering the device.
@@ -328,8 +329,8 @@ EXPORT_SYMBOL(ether_setup);
  * this private data area.
  */
 
-struct net_device *alloc_etherdev(int sizeof_priv)
+struct net_device *alloc_etherdev_mq(int sizeof_priv, unsigned int queue_count)
 {
-        return alloc_netdev(sizeof_priv, "eth%d", ether_setup);
+        return alloc_netdev_mq(sizeof_priv, "eth%d", ether_setup, queue_count);
 }
-EXPORT_SYMBOL(alloc_etherdev);
+EXPORT_SYMBOL(alloc_etherdev_mq);
diff --git a/net/sched/sch_teql.c b/net/sched/sch_teql.c
index f05ad9a30b..dfe7e45209 100644
--- a/net/sched/sch_teql.c
+++ b/net/sched/sch_teql.c
@@ -277,6 +277,7 @@ static int teql_master_xmit(struct sk_buff *skb, struct net_device *dev)
         int busy;
         int nores;
         int len = skb->len;
+        int subq = skb->queue_mapping;
         struct sk_buff *skb_res = NULL;
 
         start = master->slaves;
@@ -293,7 +294,9 @@ restart:
 
                 if (slave->qdisc_sleeping != q)
                         continue;
-                if (netif_queue_stopped(slave) || ! netif_running(slave)) {
+                if (netif_queue_stopped(slave) ||
+                    netif_subqueue_stopped(slave, subq) ||
+                    !netif_running(slave)) {
                         busy = 1;
                         continue;
                 }
@@ -302,6 +305,7 @@ restart:
                 case 0:
                         if (netif_tx_trylock(slave)) {
                                 if (!netif_queue_stopped(slave) &&
+                                    !netif_subqueue_stopped(slave, subq) &&
                                     slave->hard_start_xmit(skb, slave) == 0) {
                                         netif_tx_unlock(slave);
                                         master->slaves = NEXT_SLAVE(q);