pktgen: do not access flows[] beyond its length
[safe/jmp/linux-2.6] / net / core / pktgen.c
index d7c30ce..0666a82 100644 (file)
@@ -6,7 +6,7 @@
  *
  * Alexey Kuznetsov  <kuznet@ms2.inr.ac.ru>
  * Ben Greear <greearb@candelatech.com>
- * Jens Låås <jens.laas@data.slu.se>
+ * Jens Låås <jens.laas@data.slu.se>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
 #endif
 #include <asm/byteorder.h>
 #include <linux/rcupdate.h>
-#include <asm/bitops.h>
+#include <linux/bitops.h>
 #include <asm/io.h>
 #include <asm/dma.h>
 #include <asm/uaccess.h>
 #include <asm/div64.h>         /* do_div */
 #include <asm/timex.h>
 
-#define VERSION  "pktgen v2.69: Packet Generator for packet performance testing.\n"
+#define VERSION  "pktgen v2.70: Packet Generator for packet performance testing.\n"
 
-/* The buckets are exponential in 'width' */
-#define LAT_BUCKETS_MAX 32
 #define IP_NAME_SZ 32
 #define MAX_MPLS_LABELS 16 /* This is the max label stack depth */
 #define MPLS_STACK_BOTTOM htonl(0x00000100)
 #define F_FLOW_SEQ    (1<<11)  /* Sequential flows */
 #define F_IPSEC_ON    (1<<12)  /* ipsec on for flows */
 #define F_QUEUE_MAP_RND (1<<13)        /* queue map Random */
+#define F_QUEUE_MAP_CPU (1<<14)        /* queue map mirrors smp_processor_id() */
 
 /* Thread control flag bits */
 #define T_TERMINATE   (1<<0)
@@ -392,67 +391,12 @@ struct pktgen_thread {
        int cpu;
 
        wait_queue_head_t queue;
+       struct completion start_done;
 };
 
 #define REMOVE 1
 #define FIND   0
 
-/*  This code works around the fact that do_div cannot handle two 64-bit
-    numbers, and regular 64-bit division doesn't work on x86 kernels.
-    --Ben
-*/
-
-#define PG_DIV 0
-
-/* This was emailed to LMKL by: Chris Caputo <ccaputo@alt.net>
- * Function copied/adapted/optimized from:
- *
- *  nemesis.sourceforge.net/browse/lib/static/intmath/ix86/intmath.c.html
- *
- * Copyright 1994, University of Cambridge Computer Laboratory
- * All Rights Reserved.
- *
- */
-static inline s64 divremdi3(s64 x, s64 y, int type)
-{
-       u64 a = (x < 0) ? -x : x;
-       u64 b = (y < 0) ? -y : y;
-       u64 res = 0, d = 1;
-
-       if (b > 0) {
-               while (b < a) {
-                       b <<= 1;
-                       d <<= 1;
-               }
-       }
-
-       do {
-               if (a >= b) {
-                       a -= b;
-                       res += d;
-               }
-               b >>= 1;
-               d >>= 1;
-       }
-       while (d);
-
-       if (PG_DIV == type) {
-               return (((x ^ y) & (1ll << 63)) == 0) ? res : -(s64) res;
-       } else {
-               return ((x & (1ll << 63)) == 0) ? a : -(s64) a;
-       }
-}
-
-/* End of hacks to deal with 64-bit math on x86 */
-
-/** Convert to milliseconds */
-static inline __u64 tv_to_ms(const struct timeval *tv)
-{
-       __u64 ms = tv->tv_usec / 1000;
-       ms += (__u64) tv->tv_sec * (__u64) 1000;
-       return ms;
-}
-
 /** Convert to micro-seconds */
 static inline __u64 tv_to_us(const struct timeval *tv)
 {
@@ -461,51 +405,13 @@ static inline __u64 tv_to_us(const struct timeval *tv)
        return us;
 }
 
-static inline __u64 pg_div(__u64 n, __u32 base)
-{
-       __u64 tmp = n;
-       do_div(tmp, base);
-       /* printk("pktgen: pg_div, n: %llu  base: %d  rv: %llu\n",
-          n, base, tmp); */
-       return tmp;
-}
-
-static inline __u64 pg_div64(__u64 n, __u64 base)
-{
-       __u64 tmp = n;
-/*
- * How do we know if the architecture we are running on
- * supports division with 64 bit base?
- *
- */
-#if defined(__sparc_v9__) || defined(__powerpc64__) || defined(__alpha__) || defined(__x86_64__) || defined(__ia64__)
-
-       do_div(tmp, base);
-#else
-       tmp = divremdi3(n, base, PG_DIV);
-#endif
-       return tmp;
-}
-
-static inline __u64 getCurMs(void)
-{
-       struct timeval tv;
-       do_gettimeofday(&tv);
-       return tv_to_ms(&tv);
-}
-
-static inline __u64 getCurUs(void)
+static __u64 getCurUs(void)
 {
        struct timeval tv;
        do_gettimeofday(&tv);
        return tv_to_us(&tv);
 }
 
-static inline __u64 tv_diff(const struct timeval *a, const struct timeval *b)
-{
-       return tv_to_us(a) - tv_to_us(b);
-}
-
 /* old include end */
 
 static char version[] __initdata = VERSION;
@@ -516,6 +422,7 @@ static struct pktgen_dev *pktgen_find_dev(struct pktgen_thread *t,
                                          const char *ifname);
 static int pktgen_device_event(struct notifier_block *, unsigned long, void *);
 static void pktgen_run_all_threads(void);
+static void pktgen_reset_all_threads(void);
 static void pktgen_stop_all_threads_ifs(void);
 static int pktgen_stop_device(struct pktgen_dev *pkt_dev);
 static void pktgen_stop(struct pktgen_thread *t);
@@ -574,6 +481,9 @@ static ssize_t pgctrl_write(struct file *file, const char __user * buf,
        else if (!strcmp(data, "start"))
                pktgen_run_all_threads();
 
+       else if (!strcmp(data, "reset"))
+               pktgen_reset_all_threads();
+
        else
                printk(KERN_WARNING "pktgen: Unknown command: %s\n", data);
 
@@ -599,7 +509,6 @@ static const struct file_operations pktgen_fops = {
 
 static int pktgen_if_show(struct seq_file *seq, void *v)
 {
-       int i;
        struct pktgen_dev *pkt_dev = seq->private;
        __u64 sa;
        __u64 stopped;
@@ -648,19 +557,12 @@ static int pktgen_if_show(struct seq_file *seq, void *v)
 
        seq_puts(seq, "     src_mac: ");
 
-       if (is_zero_ether_addr(pkt_dev->src_mac))
-               for (i = 0; i < 6; i++)
-                       seq_printf(seq, "%02X%s", pkt_dev->odev->dev_addr[i],
-                                  i == 5 ? "  " : ":");
-       else
-               for (i = 0; i < 6; i++)
-                       seq_printf(seq, "%02X%s", pkt_dev->src_mac[i],
-                                  i == 5 ? "  " : ":");
+       seq_printf(seq, "%pM ",
+                  is_zero_ether_addr(pkt_dev->src_mac) ?
+                            pkt_dev->odev->dev_addr : pkt_dev->src_mac);
 
        seq_printf(seq, "dst_mac: ");
-       for (i = 0; i < 6; i++)
-               seq_printf(seq, "%02X%s", pkt_dev->dst_mac[i],
-                          i == 5 ? "\n" : ":");
+       seq_printf(seq, "%pM\n", pkt_dev->dst_mac);
 
        seq_printf(seq,
                   "     udp_src_min: %d  udp_src_max: %d  udp_dst_min: %d  udp_dst_max: %d\n",
@@ -723,6 +625,9 @@ static int pktgen_if_show(struct seq_file *seq, void *v)
        if (pkt_dev->flags & F_QUEUE_MAP_RND)
                seq_printf(seq,  "QUEUE_MAP_RND  ");
 
+       if (pkt_dev->flags & F_QUEUE_MAP_CPU)
+               seq_printf(seq,  "QUEUE_MAP_CPU  ");
+
        if (pkt_dev->cflows) {
                if (pkt_dev->flags & F_FLOW_SEQ)
                        seq_printf(seq,  "FLOW_SEQ  "); /*in sequence flows*/
@@ -1236,6 +1141,12 @@ static ssize_t pktgen_if_write(struct file *file,
 
                else if (strcmp(f, "!QUEUE_MAP_RND") == 0)
                        pkt_dev->flags &= ~F_QUEUE_MAP_RND;
+
+               else if (strcmp(f, "QUEUE_MAP_CPU") == 0)
+                       pkt_dev->flags |= F_QUEUE_MAP_CPU;
+
+               else if (strcmp(f, "!QUEUE_MAP_CPU") == 0)
+                       pkt_dev->flags &= ~F_QUEUE_MAP_CPU;
 #ifdef CONFIG_XFRM
                else if (strcmp(f, "IPSEC") == 0)
                        pkt_dev->flags |= F_IPSEC_ON;
@@ -1570,15 +1481,17 @@ static ssize_t pktgen_if_write(struct file *file,
        }
 
        if (!strcmp(name, "mpls")) {
-               unsigned n, offset;
+               unsigned n, cnt;
+
                len = get_labels(&user_buffer[i], pkt_dev);
-               if (len < 0) { return len; }
+               if (len < 0)
+                       return len;
                i += len;
-               offset = sprintf(pg_result, "OK: mpls=");
+               cnt = sprintf(pg_result, "OK: mpls=");
                for (n = 0; n < pkt_dev->nr_labels; n++)
-                       offset += sprintf(pg_result + offset,
-                                         "%08x%s", ntohl(pkt_dev->labels[n]),
-                                         n == pkt_dev->nr_labels-1 ? "" : ",");
+                       cnt += sprintf(pg_result + cnt,
+                                      "%08x%s", ntohl(pkt_dev->labels[n]),
+                                      n == pkt_dev->nr_labels-1 ? "" : ",");
 
                if (pkt_dev->nr_labels && pkt_dev->vlan_id != 0xffff) {
                        pkt_dev->vlan_id = 0xffff; /* turn off VLAN/SVLAN */
@@ -1975,7 +1888,7 @@ static int pktgen_device_event(struct notifier_block *unused,
 {
        struct net_device *dev = ptr;
 
-       if (dev->nd_net != &init_net)
+       if (!net_eq(dev_net(dev), &init_net))
                return NOTIFY_DONE;
 
        /* It is OK that we do not hold the group lock right now,
@@ -1995,6 +1908,23 @@ static int pktgen_device_event(struct notifier_block *unused,
        return NOTIFY_DONE;
 }
 
+static struct net_device *pktgen_dev_get_by_name(struct pktgen_dev *pkt_dev, const char *ifname)
+{
+       char b[IFNAMSIZ+5];
+       int i = 0;
+
+       for(i=0; ifname[i] != '@'; i++) {
+               if(i == IFNAMSIZ)
+                       break;
+
+               b[i] = ifname[i];
+       }
+       b[i] = 0;
+
+       return dev_get_by_name(&init_net, b);
+}
+
+
 /* Associate pktgen_dev with a device. */
 
 static int pktgen_setup_dev(struct pktgen_dev *pkt_dev, const char *ifname)
@@ -2008,7 +1938,7 @@ static int pktgen_setup_dev(struct pktgen_dev *pkt_dev, const char *ifname)
                pkt_dev->odev = NULL;
        }
 
-       odev = dev_get_by_name(ifname);
+       odev = pktgen_dev_get_by_name(pkt_dev, ifname);
        if (!odev) {
                printk(KERN_ERR "pktgen: no such netdevice: \"%s\"\n", ifname);
                return -ENODEV;
@@ -2034,6 +1964,8 @@ static int pktgen_setup_dev(struct pktgen_dev *pkt_dev, const char *ifname)
  */
 static void pktgen_setup_inject(struct pktgen_dev *pkt_dev)
 {
+       int ntxq;
+
        if (!pkt_dev->odev) {
                printk(KERN_ERR "pktgen: ERROR: pkt_dev->odev == NULL in "
                       "setup_inject.\n");
@@ -2042,6 +1974,26 @@ static void pktgen_setup_inject(struct pktgen_dev *pkt_dev)
                return;
        }
 
+       /* make sure that we don't pick a non-existing transmit queue */
+       ntxq = pkt_dev->odev->real_num_tx_queues;
+
+       if (ntxq <= pkt_dev->queue_map_min) {
+               printk(KERN_WARNING "pktgen: WARNING: Requested "
+                      "queue_map_min (zero-based) (%d) exceeds valid range "
+                      "[0 - %d] for (%d) queues on %s, resetting\n",
+                      pkt_dev->queue_map_min, (ntxq ?: 1)- 1, ntxq,
+                      pkt_dev->odev->name);
+               pkt_dev->queue_map_min = ntxq - 1;
+       }
+       if (pkt_dev->queue_map_max >= ntxq) {
+               printk(KERN_WARNING "pktgen: WARNING: Requested "
+                      "queue_map_max (zero-based) (%d) exceeds valid range "
+                      "[0 - %d] for (%d) queues on %s, resetting\n",
+                      pkt_dev->queue_map_max, (ntxq ?: 1)- 1, ntxq,
+                      pkt_dev->odev->name);
+               pkt_dev->queue_map_max = ntxq - 1;
+       }
+
        /* Default to the interface's mac if not explicitly set. */
 
        if (is_zero_ether_addr(pkt_dev->src_mac))
@@ -2143,7 +2095,6 @@ static void spin(struct pktgen_dev *pkt_dev, __u64 spin_until_us)
        __u64 now;
 
        start = now = getCurUs();
-       printk(KERN_INFO "sleeping for %d\n", (int)(spin_until_us - now));
        while (now < spin_until_us) {
                /* TODO: optimize sleeping behavior */
                if (spin_until_us - now > jiffies_to_usecs(1) + 1)
@@ -2186,15 +2137,19 @@ static inline int f_pick(struct pktgen_dev *pkt_dev)
                if (pkt_dev->flows[flow].count >= pkt_dev->lflow) {
                        /* reset time */
                        pkt_dev->flows[flow].count = 0;
+                       pkt_dev->flows[flow].flags = 0;
                        pkt_dev->curfl += 1;
                        if (pkt_dev->curfl >= pkt_dev->cflows)
                                pkt_dev->curfl = 0; /*reset */
                }
        } else {
                flow = random32() % pkt_dev->cflows;
+               pkt_dev->curfl = flow;
 
-               if (pkt_dev->flows[flow].count > pkt_dev->lflow)
+               if (pkt_dev->flows[flow].count > pkt_dev->lflow) {
                        pkt_dev->flows[flow].count = 0;
+                       pkt_dev->flows[flow].flags = 0;
+               }
        }
 
        return pkt_dev->curfl;
@@ -2210,7 +2165,8 @@ static void get_ipsec_sa(struct pktgen_dev *pkt_dev, int flow)
        struct xfrm_state *x = pkt_dev->flows[flow].x;
        if (!x) {
                /*slow path: we dont already have xfrm_state*/
-               x = xfrm_stateonly_find((xfrm_address_t *)&pkt_dev->cur_daddr,
+               x = xfrm_stateonly_find(&init_net,
+                                       (xfrm_address_t *)&pkt_dev->cur_daddr,
                                        (xfrm_address_t *)&pkt_dev->cur_saddr,
                                        AF_INET,
                                        pkt_dev->ipsmode,
@@ -2224,6 +2180,29 @@ 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->flags & F_QUEUE_MAP_CPU)
+               pkt_dev->cur_queue_map = smp_processor_id();
+
+       else 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;
+       }
+       pkt_dev->cur_queue_map  = pkt_dev->cur_queue_map % pkt_dev->odev->real_num_tx_queues;
+}
+
 /* Increment/randomize headers according to flags and current values
  * for IP src/dest, UDP src/dst port, MAC-Addr src/dst
  */
@@ -2245,7 +2224,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
                        mc = random32() % pkt_dev->src_mac_count;
                else {
                        mc = pkt_dev->cur_src_mac_offset++;
-                       if (pkt_dev->cur_src_mac_offset >
+                       if (pkt_dev->cur_src_mac_offset >=
                            pkt_dev->src_mac_count)
                                pkt_dev->cur_src_mac_offset = 0;
                }
@@ -2272,7 +2251,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 
                else {
                        mc = pkt_dev->cur_dst_mac_offset++;
-                       if (pkt_dev->cur_dst_mac_offset >
+                       if (pkt_dev->cur_dst_mac_offset >=
                            pkt_dev->dst_mac_count) {
                                pkt_dev->cur_dst_mac_offset = 0;
                        }
@@ -2363,9 +2342,11 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
                                        t = random32() % (imx - imn) + imn;
                                        s = htonl(t);
 
-                                       while (LOOPBACK(s) || MULTICAST(s)
-                                              || BADCLASS(s) || ZERONET(s)
-                                              || LOCAL_MCAST(s)) {
+                                       while (ipv4_is_loopback(s) ||
+                                              ipv4_is_multicast(s) ||
+                                              ipv4_is_lbcast(s) ||
+                                              ipv4_is_zeronet(s) ||
+                                              ipv4_is_local_multicast(s)) {
                                                t = random32() % (imx - imn) + imn;
                                                s = htonl(t);
                                        }
@@ -2424,19 +2405,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++;
 }
@@ -2459,7 +2428,7 @@ static int pktgen_output_ipsec(struct sk_buff *skb, struct pktgen_dev *pkt_dev)
        spin_lock(&x->lock);
        iph = ip_hdr(skb);
 
-       err = x->mode->output(x, skb);
+       err = x->outer_mode->output(x, skb);
        if (err)
                goto error;
        err = x->type->output(x, skb);
@@ -2468,8 +2437,6 @@ static int pktgen_output_ipsec(struct sk_buff *skb, struct pktgen_dev *pkt_dev)
 
        x->curlft.bytes +=skb->len;
        x->curlft.packets++;
-       spin_unlock(&x->lock);
-
 error:
        spin_unlock(&x->lock);
        return err;
@@ -2480,7 +2447,7 @@ static inline void free_SAs(struct pktgen_dev *pkt_dev)
        if (pkt_dev->cflows) {
                /* let go of the SAs if we have them */
                int i = 0;
-               for (;  i < pkt_dev->nflows; i++){
+               for (;  i < pkt_dev->cflows; i++) {
                        struct xfrm_state *x = pkt_dev->flows[i].x;
                        if (x) {
                                xfrm_state_put(x);
@@ -2505,7 +2472,7 @@ static inline int process_ipsec(struct pktgen_dev *pkt_dev,
                                if (ret < 0) {
                                        printk(KERN_ERR "Error expanding "
                                               "ipsec packet %d\n",ret);
-                                       return 0;
+                                       goto err;
                                }
                        }
 
@@ -2515,8 +2482,7 @@ static inline int process_ipsec(struct pktgen_dev *pkt_dev,
                        if (ret) {
                                printk(KERN_ERR "Error creating ipsec "
                                       "packet %d\n",ret);
-                               kfree_skb(skb);
-                               return 0;
+                               goto err;
                        }
                        /* restore ll */
                        eth = (__u8 *) skb_push(skb, ETH_HLEN);
@@ -2525,6 +2491,9 @@ static inline int process_ipsec(struct pktgen_dev *pkt_dev,
                }
        }
        return 1;
+err:
+       kfree_skb(skb);
+       return 0;
 }
 #endif
 
@@ -2559,7 +2528,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);
@@ -2570,6 +2539,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;
@@ -2608,8 +2578,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->queue_mapping = pkt_dev->cur_queue_map;
-
+       skb_set_queue_mapping(skb, queue_map);
        iph = ip_hdr(skb);
        udph = udp_hdr(skb);
 
@@ -2738,6 +2707,7 @@ static unsigned int scan_ip6(const char *s, char ip[16])
        unsigned int prefixlen = 0;
        unsigned int suffixlen = 0;
        __be32 tmp;
+       char *pos;
 
        for (i = 0; i < 16; i++)
                ip[i] = 0;
@@ -2752,12 +2722,9 @@ static unsigned int scan_ip6(const char *s, char ip[16])
                        }
                        s++;
                }
-               {
-                       char *tmp;
-                       u = simple_strtoul(s, &tmp, 16);
-                       i = tmp - s;
-               }
 
+               u = simple_strtoul(s, &pos, 16);
+               i = pos - s;
                if (!i)
                        return 0;
                if (prefixlen == 12 && s[i] == '.') {
@@ -2785,11 +2752,9 @@ static unsigned int scan_ip6(const char *s, char ip[16])
                        len++;
                } else if (suffixlen != 0)
                        break;
-               {
-                       char *tmp;
-                       u = simple_strtol(s, &tmp, 16);
-                       i = tmp - s;
-               }
+
+               u = simple_strtol(s, &pos, 16);
+               i = pos - s;
                if (!i) {
                        if (*s)
                                len--;
@@ -2903,6 +2868,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);
@@ -2913,6 +2879,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 +
@@ -2950,8 +2917,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->queue_mapping = pkt_dev->cur_queue_map;
-
+       skb_set_queue_mapping(skb, queue_map);
        iph = ipv6_hdr(skb);
        udph = udp_hdr(skb);
 
@@ -3207,6 +3173,24 @@ static void pktgen_run_all_threads(void)
        pktgen_wait_all_threads_run();
 }
 
+static void pktgen_reset_all_threads(void)
+{
+       struct pktgen_thread *t;
+
+       pr_debug("pktgen: entering pktgen_reset_all_threads.\n");
+
+       mutex_lock(&pktgen_thread_lock);
+
+       list_for_each_entry(t, &pktgen_threads, th_list)
+               t->control |= (T_REMDEVALL);
+
+       mutex_unlock(&pktgen_thread_lock);
+
+       schedule_timeout_interruptible(msecs_to_jiffies(125));  /* Propagate thread->control  */
+
+       pktgen_wait_all_threads_run();
+}
+
 static void show_results(struct pktgen_dev *pkt_dev, int nr_frags)
 {
        __u64 total_us, bps, mbps, pps, idle;
@@ -3291,8 +3275,7 @@ static void pktgen_stop(struct pktgen_thread *t)
 
        list_for_each_entry(pkt_dev, &t->if_list, list) {
                pktgen_stop_device(pkt_dev);
-               if (pkt_dev->skb)
-                       kfree_skb(pkt_dev->skb);
+               kfree_skb(pkt_dev->skb);
 
                pkt_dev->skb = NULL;
        }
@@ -3319,8 +3302,7 @@ static void pktgen_rem_one_if(struct pktgen_thread *t)
                if (!cur->removal_mark)
                        continue;
 
-               if (cur->skb)
-                       kfree_skb(cur->skb);
+               kfree_skb(cur->skb);
                cur->skb = NULL;
 
                pktgen_remove_device(t, cur);
@@ -3344,8 +3326,7 @@ static void pktgen_rem_all_ifs(struct pktgen_thread *t)
        list_for_each_safe(q, n, &t->if_list) {
                cur = list_entry(q, struct pktgen_dev, list);
 
-               if (cur->skb)
-                       kfree_skb(cur->skb);
+               kfree_skb(cur->skb);
                cur->skb = NULL;
 
                pktgen_remove_device(t, cur);
@@ -3369,12 +3350,14 @@ 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 net_device *odev = pkt_dev->odev;
+       int (*xmit)(struct sk_buff *, struct net_device *)
+               = odev->netdev_ops->ndo_start_xmit;
+       struct netdev_queue *txq;
        __u64 idle_start = 0;
+       u16 queue_map;
        int ret;
 
-       odev = pkt_dev->odev;
-
        if (pkt_dev->delay_us || pkt_dev->delay_ns) {
                u64 now;
 
@@ -3392,16 +3375,22 @@ 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->queue_mapping))) ||
+       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) ||
+           netif_tx_queue_frozen(txq) ||
            need_resched()) {
                idle_start = getCurUs();
 
                if (!netif_running(odev)) {
                        pktgen_stop_device(pkt_dev);
-                       if (pkt_dev->skb)
-                               kfree_skb(pkt_dev->skb);
+                       kfree_skb(pkt_dev->skb);
                        pkt_dev->skb = NULL;
                        goto out;
                }
@@ -3410,8 +3399,8 @@ 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->queue_mapping)) {
+               if (netif_tx_queue_stopped(txq) ||
+                   netif_tx_queue_frozen(txq)) {
                        pkt_dev->next_tx_us = getCurUs();       /* TODO */
                        pkt_dev->next_tx_ns = 0;
                        goto out;       /* Try the next interface */
@@ -3422,8 +3411,7 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
                if ((++pkt_dev->clone_count >= pkt_dev->clone_skb)
                    || (!pkt_dev->skb)) {
                        /* build a new pkt */
-                       if (pkt_dev->skb)
-                               kfree_skb(pkt_dev->skb);
+                       kfree_skb(pkt_dev->skb);
 
                        pkt_dev->skb = fill_packet(odev, pkt_dev);
                        if (pkt_dev->skb == NULL) {
@@ -3438,13 +3426,17 @@ 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->queue_mapping)) {
+       /* 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) &&
+           !netif_tx_queue_frozen(txq)) {
 
                atomic_inc(&(pkt_dev->skb->users));
              retry_now:
-               ret = odev->hard_start_xmit(pkt_dev->skb, odev);
+               ret = (*xmit)(pkt_dev->skb, odev);
                if (likely(ret == NETDEV_TX_OK)) {
                        pkt_dev->last_ok = 1;
                        pkt_dev->sofar++;
@@ -3484,7 +3476,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)) {
@@ -3501,8 +3493,7 @@ static __inline__ void pktgen_xmit(struct pktgen_dev *pkt_dev)
 
                /* Done with this */
                pktgen_stop_device(pkt_dev);
-               if (pkt_dev->skb)
-                       kfree_skb(pkt_dev->skb);
+               kfree_skb(pkt_dev->skb);
                pkt_dev->skb = NULL;
        }
 out:;
@@ -3522,8 +3513,9 @@ static int pktgen_thread_worker(void *arg)
        BUG_ON(smp_processor_id() != cpu);
 
        init_waitqueue_head(&t->queue);
+       complete(&t->start_done);
 
-       pr_debug("pktgen: starting pktgen/%d:  pid=%d\n", cpu, current->pid);
+       pr_debug("pktgen: starting pktgen/%d:  pid=%d\n", cpu, task_pid_nr(current));
 
        set_current_state(TASK_INTERRUPTIBLE);
 
@@ -3678,15 +3670,14 @@ static int pktgen_add_device(struct pktgen_thread *t, const char *ifname)
        if (err)
                goto out1;
 
-       pkt_dev->entry = create_proc_entry(ifname, 0600, pg_proc_dir);
+       pkt_dev->entry = proc_create_data(ifname, 0600, pg_proc_dir,
+                                         &pktgen_if_fops, pkt_dev);
        if (!pkt_dev->entry) {
                printk(KERN_ERR "pktgen: cannot create %s/%s procfs entry.\n",
                       PG_PROC_DIR, ifname);
                err = -EINVAL;
                goto out2;
        }
-       pkt_dev->entry->proc_fops = &pktgen_if_fops;
-       pkt_dev->entry->data = pkt_dev;
 #ifdef CONFIG_XFRM
        pkt_dev->ipsmode = XFRM_MODE_TRANSPORT;
        pkt_dev->ipsproto = IPPROTO_ESP;
@@ -3724,6 +3715,7 @@ static int __init pktgen_create_thread(int cpu)
        INIT_LIST_HEAD(&t->if_list);
 
        list_add_tail(&t->th_list, &pktgen_threads);
+       init_completion(&t->start_done);
 
        p = kthread_create(pktgen_thread_worker, t, "kpktgend_%d", cpu);
        if (IS_ERR(p)) {
@@ -3736,7 +3728,8 @@ static int __init pktgen_create_thread(int cpu)
        kthread_bind(p, cpu);
        t->tsk = p;
 
-       pe = create_proc_entry(t->tsk->comm, 0600, pg_proc_dir);
+       pe = proc_create_data(t->tsk->comm, 0600, pg_proc_dir,
+                             &pktgen_thread_fops, t);
        if (!pe) {
                printk(KERN_ERR "pktgen: cannot create %s/%s procfs entry.\n",
                       PG_PROC_DIR, t->tsk->comm);
@@ -3746,10 +3739,8 @@ static int __init pktgen_create_thread(int cpu)
                return -EINVAL;
        }
 
-       pe->proc_fops = &pktgen_thread_fops;
-       pe->data = t;
-
        wake_up_process(p);
+       wait_for_completion(&t->start_done);
 
        return 0;
 }
@@ -3815,9 +3806,8 @@ static int __init pg_init(void)
        pg_proc_dir = proc_mkdir(PG_PROC_DIR, init_net.proc_net);
        if (!pg_proc_dir)
                return -ENODEV;
-       pg_proc_dir->owner = THIS_MODULE;
 
-       pe = create_proc_entry(PGCTRL, 0600, pg_proc_dir);
+       pe = proc_create(PGCTRL, 0600, pg_proc_dir, &pktgen_fops);
        if (pe == NULL) {
                printk(KERN_ERR "pktgen: ERROR: cannot create %s "
                       "procfs entry.\n", PGCTRL);
@@ -3825,9 +3815,6 @@ static int __init pg_init(void)
                return -EINVAL;
        }
 
-       pe->proc_fops = &pktgen_fops;
-       pe->data = NULL;
-
        /* Register us to receive netdevice events */
        register_netdevice_notifier(&pktgen_notifier_block);