1 files changed, 176 insertions, 331 deletions

diff --git a/net/core/pktgen.c b/net/core/pktgen.c
index 10a1ea72010d..f76079cd750c 100644
--- a/net/core/pktgen.c
+++ b/net/core/pktgen.c
@@ -156,6 +156,7 @@
 #include <linux/wait.h>
 #include <linux/etherdevice.h>
 #include <linux/kthread.h>
+#include <linux/prefetch.h>
 #include <net/net_namespace.h>
 #include <net/checksum.h>
 #include <net/ipv6.h>
@@ -251,6 +252,7 @@ struct pktgen_dev {
         int max_pkt_size;       /* = ETH_ZLEN; */
         int pkt_overhead;       /* overhead for MPLS, VLANs, IPSEC etc */
         int nfrags;
+        struct page *page;
         u64 delay;              /* nano-seconds */
 
         __u64 count;            /* Default No packets to send */
@@ -378,6 +380,7 @@ struct pktgen_dev {
 
         u16 queue_map_min;
         u16 queue_map_max;
+        __u32 skb_priority;     /* skb priority field */
         int node;               /* Memory node */
 
 #ifdef CONFIG_XFRM
@@ -394,6 +397,8 @@ struct pktgen_hdr {
         __be32 tv_usec;
 };
 
+static bool pktgen_exiting __read_mostly;
+
 struct pktgen_thread {
         spinlock_t if_lock;             /* for list of devices */
         struct list_head if_list;       /* All device here */
@@ -445,7 +450,6 @@ static void pktgen_stop(struct pktgen_thread *t);
 static void pktgen_clear_counters(struct pktgen_dev *pkt_dev);
 
 static unsigned int scan_ip6(const char *s, char ip[16]);
-static unsigned int fmt_ip6(char *s, const char ip[16]);
 
 /* Module parameters, defaults. */
 static int pg_count_d __read_mostly = 1000;
@@ -547,22 +551,18 @@ static int pktgen_if_show(struct seq_file *seq, void *v)
                    pkt_dev->queue_map_min,
                    pkt_dev->queue_map_max);
 
-        if (pkt_dev->flags & F_IPV6) {
-                char b1[128], b2[128], b3[128];
-                fmt_ip6(b1, pkt_dev->in6_saddr.s6_addr);
-                fmt_ip6(b2, pkt_dev->min_in6_saddr.s6_addr);
-                fmt_ip6(b3, pkt_dev->max_in6_saddr.s6_addr);
-                seq_printf(seq,
-                           "     saddr: %s  min_saddr: %s  max_saddr: %s\n", b1,
-                           b2, b3);
+        if (pkt_dev->skb_priority)
+                seq_printf(seq, "     skb_priority: %u\n",
+                           pkt_dev->skb_priority);
 
-                fmt_ip6(b1, pkt_dev->in6_daddr.s6_addr);
-                fmt_ip6(b2, pkt_dev->min_in6_daddr.s6_addr);
-                fmt_ip6(b3, pkt_dev->max_in6_daddr.s6_addr);
+        if (pkt_dev->flags & F_IPV6) {
                 seq_printf(seq,
-                           "     daddr: %s  min_daddr: %s  max_daddr: %s\n", b1,
-                           b2, b3);
-
+                           "     saddr: %pI6c  min_saddr: %pI6c  max_saddr: %pI6c\n"
+                           "     daddr: %pI6c  min_daddr: %pI6c  max_daddr: %pI6c\n",
+                           &pkt_dev->in6_saddr,
+                           &pkt_dev->min_in6_saddr, &pkt_dev->max_in6_saddr,
+                           &pkt_dev->in6_daddr,
+                           &pkt_dev->min_in6_daddr, &pkt_dev->max_in6_daddr);
         } else {
                 seq_printf(seq,
                            "     dst_min: %s  dst_max: %s\n",
@@ -698,10 +698,9 @@ static int pktgen_if_show(struct seq_file *seq, void *v)
                    pkt_dev->cur_src_mac_offset);
 
         if (pkt_dev->flags & F_IPV6) {
-                char b1[128], b2[128];
-                fmt_ip6(b1, pkt_dev->cur_in6_daddr.s6_addr);
-                fmt_ip6(b2, pkt_dev->cur_in6_saddr.s6_addr);
-                seq_printf(seq, "     cur_saddr: %s  cur_daddr: %s\n", b2, b1);
+                seq_printf(seq, "     cur_saddr: %pI6c  cur_daddr: %pI6c\n",
+                                &pkt_dev->cur_in6_saddr,
+                                &pkt_dev->cur_in6_daddr);
         } else
                 seq_printf(seq, "     cur_saddr: 0x%x  cur_daddr: 0x%x\n",
                            pkt_dev->cur_saddr, pkt_dev->cur_daddr);
@@ -729,16 +728,14 @@ static int hex32_arg(const char __user *user_buffer, unsigned long maxlen,
         *num = 0;
 
         for (; i < maxlen; i++) {
+                int value;
                 char c;
                 *num <<= 4;
                 if (get_user(c, &user_buffer[i]))
                         return -EFAULT;
-                if ((c >= '0') && (c <= '9'))
-                        *num |= c - '0';
-                else if ((c >= 'a') && (c <= 'f'))
-                        *num |= c - 'a' + 10;
-                else if ((c >= 'A') && (c <= 'F'))
-                        *num |= c - 'A' + 10;
+                value = hex_to_bin(c);
+                if (value >= 0)
+                        *num |= value;
                 else
                         break;
         }
@@ -773,10 +770,10 @@ done:
 static unsigned long num_arg(const char __user * user_buffer,
                              unsigned long maxlen, unsigned long *num)
 {
-        int i = 0;
+        int i;
         *num = 0;
 
-        for (; i < maxlen; i++) {
+        for (i = 0; i < maxlen; i++) {
                 char c;
                 if (get_user(c, &user_buffer[i]))
                         return -EFAULT;
@@ -791,9 +788,9 @@ static unsigned long num_arg(const char __user * user_buffer,
 
 static int strn_len(const char __user * user_buffer, unsigned int maxlen)
 {
-        int i = 0;
+        int i;
 
-        for (; i < maxlen; i++) {
+        for (i = 0; i < maxlen; i++) {
                 char c;
                 if (get_user(c, &user_buffer[i]))
                         return -EFAULT;
@@ -848,7 +845,7 @@ static ssize_t pktgen_if_write(struct file *file,
 {
         struct seq_file *seq = file->private_data;
         struct pktgen_dev *pkt_dev = seq->private;
-        int i = 0, max, len;
+        int i, max, len;
         char name[16], valstr[32];
         unsigned long value = 0;
         char *pg_result = NULL;
@@ -862,13 +859,13 @@ static ssize_t pktgen_if_write(struct file *file,
                 return -EINVAL;
         }
 
-        max = count - i;
-        tmp = count_trail_chars(&user_buffer[i], max);
+        max = count;
+        tmp = count_trail_chars(user_buffer, max);
         if (tmp < 0) {
                 pr_warning("illegal format\n");
                 return tmp;
         }
-        i += tmp;
+        i = tmp;
 
         /* Read variable name */
 
@@ -889,10 +886,11 @@ static ssize_t pktgen_if_write(struct file *file,
         i += len;
 
         if (debug) {
-                char tb[count + 1];
-                if (copy_from_user(tb, user_buffer, count))
+                size_t copy = min_t(size_t, count, 1023);
+                char tb[copy + 1];
+                if (copy_from_user(tb, user_buffer, copy))
                         return -EFAULT;
-                tb[count] = 0;
+                tb[copy] = 0;
                 printk(KERN_DEBUG "pktgen: %s,%lu  buffer -:%s:-\n", name,
                        (unsigned long)count, tb);
         }
@@ -1128,6 +1126,10 @@ static ssize_t pktgen_if_write(struct file *file,
                 if (node_possible(value)) {
                         pkt_dev->node = value;
                         sprintf(pg_result, "OK: node=%d", pkt_dev->node);
+                        if (pkt_dev->page) {
+                                put_page(pkt_dev->page);
+                                pkt_dev->page = NULL;
+                        }
                 }
                 else
                         sprintf(pg_result, "ERROR: node not possible");
@@ -1298,7 +1300,7 @@ static ssize_t pktgen_if_write(struct file *file,
                 buf[len] = 0;
 
                 scan_ip6(buf, pkt_dev->in6_daddr.s6_addr);
-                fmt_ip6(buf, pkt_dev->in6_daddr.s6_addr);
+                snprintf(buf, sizeof(buf), "%pI6c", &pkt_dev->in6_daddr);
 
                 ipv6_addr_copy(&pkt_dev->cur_in6_daddr, &pkt_dev->in6_daddr);
 
@@ -1321,7 +1323,7 @@ static ssize_t pktgen_if_write(struct file *file,
                 buf[len] = 0;
 
                 scan_ip6(buf, pkt_dev->min_in6_daddr.s6_addr);
-                fmt_ip6(buf, pkt_dev->min_in6_daddr.s6_addr);
+                snprintf(buf, sizeof(buf), "%pI6c", &pkt_dev->min_in6_daddr);
 
                 ipv6_addr_copy(&pkt_dev->cur_in6_daddr,
                                &pkt_dev->min_in6_daddr);
@@ -1344,7 +1346,7 @@ static ssize_t pktgen_if_write(struct file *file,
                 buf[len] = 0;
 
                 scan_ip6(buf, pkt_dev->max_in6_daddr.s6_addr);
-                fmt_ip6(buf, pkt_dev->max_in6_daddr.s6_addr);
+                snprintf(buf, sizeof(buf), "%pI6c", &pkt_dev->max_in6_daddr);
 
                 if (debug)
                         printk(KERN_DEBUG "pktgen: dst6_max set to: %s\n", buf);
@@ -1365,7 +1367,7 @@ static ssize_t pktgen_if_write(struct file *file,
                 buf[len] = 0;
 
                 scan_ip6(buf, pkt_dev->in6_saddr.s6_addr);
-                fmt_ip6(buf, pkt_dev->in6_saddr.s6_addr);
+                snprintf(buf, sizeof(buf), "%pI6c", &pkt_dev->in6_saddr);
 
                 ipv6_addr_copy(&pkt_dev->cur_in6_saddr, &pkt_dev->in6_saddr);
 
@@ -1419,11 +1421,6 @@ static ssize_t pktgen_if_write(struct file *file,
                 return count;
         }
         if (!strcmp(name, "dst_mac")) {
-                char *v = valstr;
-                unsigned char old_dmac[ETH_ALEN];
-                unsigned char *m = pkt_dev->dst_mac;
-                memcpy(old_dmac, pkt_dev->dst_mac, ETH_ALEN);
-
                 len = strn_len(&user_buffer[i], sizeof(valstr) - 1);
                 if (len < 0)
                         return len;
@@ -1431,35 +1428,16 @@ static ssize_t pktgen_if_write(struct file *file,
                 memset(valstr, 0, sizeof(valstr));
                 if (copy_from_user(valstr, &user_buffer[i], len))
                         return -EFAULT;
-                i += len;
-
-                for (*m = 0; *v && m < pkt_dev->dst_mac + 6; v++) {
-                        int value;
-
-                        value = hex_to_bin(*v);
-                        if (value >= 0)
-                                *m = *m * 16 + value;
-
-                        if (*v == ':') {
-                                m++;
-                                *m = 0;
-                        }
-                }
 
+                if (!mac_pton(valstr, pkt_dev->dst_mac))
+                        return -EINVAL;
                 /* Set up Dest MAC */
-                if (compare_ether_addr(old_dmac, pkt_dev->dst_mac))
-                        memcpy(&(pkt_dev->hh[0]), pkt_dev->dst_mac, ETH_ALEN);
+                memcpy(&pkt_dev->hh[0], pkt_dev->dst_mac, ETH_ALEN);
 
-                sprintf(pg_result, "OK: dstmac");
+                sprintf(pg_result, "OK: dstmac %pM", pkt_dev->dst_mac);
                 return count;
         }
         if (!strcmp(name, "src_mac")) {
-                char *v = valstr;
-                unsigned char old_smac[ETH_ALEN];
-                unsigned char *m = pkt_dev->src_mac;
-
-                memcpy(old_smac, pkt_dev->src_mac, ETH_ALEN);
-
                 len = strn_len(&user_buffer[i], sizeof(valstr) - 1);
                 if (len < 0)
                         return len;
@@ -1467,26 +1445,13 @@ static ssize_t pktgen_if_write(struct file *file,
                 memset(valstr, 0, sizeof(valstr));
                 if (copy_from_user(valstr, &user_buffer[i], len))
                         return -EFAULT;
-                i += len;
-
-                for (*m = 0; *v && m < pkt_dev->src_mac + 6; v++) {
-                        int value;
-
-                        value = hex_to_bin(*v);
-                        if (value >= 0)
-                                *m = *m * 16 + value;
-
-                        if (*v == ':') {
-                                m++;
-                                *m = 0;
-                        }
-                }
 
+                if (!mac_pton(valstr, pkt_dev->src_mac))
+                        return -EINVAL;
                 /* Set up Src MAC */
-                if (compare_ether_addr(old_smac, pkt_dev->src_mac))
-                        memcpy(&(pkt_dev->hh[6]), pkt_dev->src_mac, ETH_ALEN);
+                memcpy(&pkt_dev->hh[6], pkt_dev->src_mac, ETH_ALEN);
 
-                sprintf(pg_result, "OK: srcmac");
+                sprintf(pg_result, "OK: srcmac %pM", pkt_dev->src_mac);
                 return count;
         }
 
@@ -1712,6 +1677,18 @@ static ssize_t pktgen_if_write(struct file *file,
                 return count;
         }
 
+        if (!strcmp(name, "skb_priority")) {
+                len = num_arg(&user_buffer[i], 9, &value);
+                if (len < 0)
+                        return len;
+
+                i += len;
+                pkt_dev->skb_priority = value;
+                sprintf(pg_result, "OK: skb_priority=%i",
+                        pkt_dev->skb_priority);
+                return count;
+        }
+
         sprintf(pkt_dev->result, "No such parameter \"%s\"", name);
         return -EINVAL;
 }
@@ -1766,7 +1743,7 @@ static ssize_t pktgen_thread_write(struct file *file,
 {
         struct seq_file *seq = file->private_data;
         struct pktgen_thread *t = seq->private;
-        int i = 0, max, len, ret;
+        int i, max, len, ret;
         char name[40];
         char *pg_result;
 
@@ -1775,12 +1752,12 @@ static ssize_t pktgen_thread_write(struct file *file,
                 return -EINVAL;
         }
 
-        max = count - i;
-        len = count_trail_chars(&user_buffer[i], max);
+        max = count;
+        len = count_trail_chars(user_buffer, max);
         if (len < 0)
                 return len;
 
-        i += len;
+        i = len;
 
         /* Read variable name */
 
@@ -1977,7 +1954,7 @@ static struct net_device *pktgen_dev_get_by_name(struct pktgen_dev *pkt_dev,
                                                  const char *ifname)
 {
         char b[IFNAMSIZ+5];
-        int i = 0;
+        int i;
 
         for (i = 0; ifname[i] != '@'; i++) {
                 if (i == IFNAMSIZ)
@@ -2491,7 +2468,6 @@ static int pktgen_output_ipsec(struct sk_buff *skb, struct pktgen_dev *pkt_dev)
 {
         struct xfrm_state *x = pkt_dev->flows[pkt_dev->curfl].x;
         int err = 0;
-        struct iphdr *iph;
 
         if (!x)
                 return 0;
@@ -2501,7 +2477,6 @@ static int pktgen_output_ipsec(struct sk_buff *skb, struct pktgen_dev *pkt_dev)
                 return 0;
 
         spin_lock(&x->lock);
-        iph = ip_hdr(skb);
 
         err = x->outer_mode->output(x, skb);
         if (err)
@@ -2521,8 +2496,8 @@ static 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->cflows; i++) {
+                int i;
+                for (i = 0; i < pkt_dev->cflows; i++) {
                         struct xfrm_state *x = pkt_dev->flows[i].x;
                         if (x) {
                                 xfrm_state_put(x);
@@ -2587,6 +2562,72 @@ static inline __be16 build_tci(unsigned int id, unsigned int cfi,
         return htons(id | (cfi << 12) | (prio << 13));
 }
 
+static void pktgen_finalize_skb(struct pktgen_dev *pkt_dev, struct sk_buff *skb,
+                                int datalen)
+{
+        struct timeval timestamp;
+        struct pktgen_hdr *pgh;
+
+        pgh = (struct pktgen_hdr *)skb_put(skb, sizeof(*pgh));
+        datalen -= sizeof(*pgh);
+
+        if (pkt_dev->nfrags <= 0) {
+                memset(skb_put(skb, datalen), 0, datalen);
+        } else {
+                int frags = pkt_dev->nfrags;
+                int i, len;
+                int frag_len;
+
+
+                if (frags > MAX_SKB_FRAGS)
+                        frags = MAX_SKB_FRAGS;
+                len = datalen - frags * PAGE_SIZE;
+                if (len > 0) {
+                        memset(skb_put(skb, len), 0, len);
+                        datalen = frags * PAGE_SIZE;
+                }
+
+                i = 0;
+                frag_len = (datalen/frags) < PAGE_SIZE ?
+                           (datalen/frags) : PAGE_SIZE;
+                while (datalen > 0) {
+                        if (unlikely(!pkt_dev->page)) {
+                                int node = numa_node_id();
+
+                                if (pkt_dev->node >= 0 && (pkt_dev->flags & F_NODE))
+                                        node = pkt_dev->node;
+                                pkt_dev->page = alloc_pages_node(node, GFP_KERNEL | __GFP_ZERO, 0);
+                                if (!pkt_dev->page)
+                                        break;
+                        }
+                        skb_shinfo(skb)->frags[i].page = pkt_dev->page;
+                        get_page(pkt_dev->page);
+                        skb_shinfo(skb)->frags[i].page_offset = 0;
+                        /*last fragment, fill rest of data*/
+                        if (i == (frags - 1))
+                                skb_shinfo(skb)->frags[i].size =
+                                    (datalen < PAGE_SIZE ? datalen : PAGE_SIZE);
+                        else
+                                skb_shinfo(skb)->frags[i].size = frag_len;
+                        datalen -= skb_shinfo(skb)->frags[i].size;
+                        skb->len += skb_shinfo(skb)->frags[i].size;
+                        skb->data_len += skb_shinfo(skb)->frags[i].size;
+                        i++;
+                        skb_shinfo(skb)->nr_frags = i;
+                }
+        }
+
+        /* Stamp the time, and sequence number,
+         * convert them to network byte order
+         */
+        pgh->pgh_magic = htonl(PKTGEN_MAGIC);
+        pgh->seq_num = htonl(pkt_dev->seq_num);
+
+        do_gettimeofday(&timestamp);
+        pgh->tv_sec = htonl(timestamp.tv_sec);
+        pgh->tv_usec = htonl(timestamp.tv_usec);
+}
+
 static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
                                         struct pktgen_dev *pkt_dev)
 {
@@ -2595,7 +2636,6 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
         struct udphdr *udph;
         int datalen, iplen;
         struct iphdr *iph;
-        struct pktgen_hdr *pgh = NULL;
         __be16 protocol = htons(ETH_P_IP);
         __be32 *mpls;
         __be16 *vlan_tci = NULL;                 /* Encapsulates priority and VLAN ID */
@@ -2613,8 +2653,8 @@ 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);
+        queue_map = pkt_dev->cur_queue_map;
 
         datalen = (odev->hard_header_len + 16) & ~0xf;
 
@@ -2642,6 +2682,7 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
                 sprintf(pkt_dev->result, "No memory");
                 return NULL;
         }
+        prefetchw(skb->data);
 
         skb_reserve(skb, datalen);
 
@@ -2672,6 +2713,8 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
         skb->transport_header = skb->network_header + sizeof(struct iphdr);
         skb_put(skb, sizeof(struct iphdr) + sizeof(struct udphdr));
         skb_set_queue_mapping(skb, queue_map);
+        skb->priority = pkt_dev->skb_priority;
+
         iph = ip_hdr(skb);
         udph = udp_hdr(skb);
 
@@ -2708,76 +2751,7 @@ static struct sk_buff *fill_packet_ipv4(struct net_device *odev,
                            pkt_dev->pkt_overhead);
         skb->dev = odev;
         skb->pkt_type = PACKET_HOST;
-
-        if (pkt_dev->nfrags <= 0) {
-                pgh = (struct pktgen_hdr *)skb_put(skb, datalen);
-                memset(pgh + 1, 0, datalen - sizeof(struct pktgen_hdr));
-        } else {
-                int frags = pkt_dev->nfrags;
-                int i, len;
-
-                pgh = (struct pktgen_hdr *)(((char *)(udph)) + 8);
-
-                if (frags > MAX_SKB_FRAGS)
-                        frags = MAX_SKB_FRAGS;
-                if (datalen > frags * PAGE_SIZE) {
-                        len = datalen - frags * PAGE_SIZE;
-                        memset(skb_put(skb, len), 0, len);
-                        datalen = frags * PAGE_SIZE;
-                }
-
-                i = 0;
-                while (datalen > 0) {
-                        struct page *page = alloc_pages(GFP_KERNEL | __GFP_ZERO, 0);
-                        skb_shinfo(skb)->frags[i].page = page;
-                        skb_shinfo(skb)->frags[i].page_offset = 0;
-                        skb_shinfo(skb)->frags[i].size =
-                            (datalen < PAGE_SIZE ? datalen : PAGE_SIZE);
-                        datalen -= skb_shinfo(skb)->frags[i].size;
-                        skb->len += skb_shinfo(skb)->frags[i].size;
-                        skb->data_len += skb_shinfo(skb)->frags[i].size;
-                        i++;
-                        skb_shinfo(skb)->nr_frags = i;
-                }
-
-                while (i < frags) {
-                        int rem;
-
-                        if (i == 0)
-                                break;
-
-                        rem = skb_shinfo(skb)->frags[i - 1].size / 2;
-                        if (rem == 0)
-                                break;
-
-                        skb_shinfo(skb)->frags[i - 1].size -= rem;
-
-                        skb_shinfo(skb)->frags[i] =
-                            skb_shinfo(skb)->frags[i - 1];
-                        get_page(skb_shinfo(skb)->frags[i].page);
-                        skb_shinfo(skb)->frags[i].page =
-                            skb_shinfo(skb)->frags[i - 1].page;
-                        skb_shinfo(skb)->frags[i].page_offset +=
-                            skb_shinfo(skb)->frags[i - 1].size;
-                        skb_shinfo(skb)->frags[i].size = rem;
-                        i++;
-                        skb_shinfo(skb)->nr_frags = i;
-                }
-        }
-
-        /* Stamp the time, and sequence number,
-         * convert them to network byte order
-         */
-        if (pgh) {
-                struct timeval timestamp;
-
-                pgh->pgh_magic = htonl(PKTGEN_MAGIC);
-                pgh->seq_num = htonl(pkt_dev->seq_num);
-
-                do_gettimeofday(&timestamp);
-                pgh->tv_sec = htonl(timestamp.tv_sec);
-                pgh->tv_usec = htonl(timestamp.tv_usec);
-        }
+        pktgen_finalize_skb(pkt_dev, skb, datalen);
 
 #ifdef CONFIG_XFRM
         if (!process_ipsec(pkt_dev, skb, protocol))
@@ -2878,79 +2852,6 @@ static unsigned int scan_ip6(const char *s, char ip[16])
         return len;
 }
 
-static char tohex(char hexdigit)
-{
-        return hexdigit > 9 ? hexdigit + 'a' - 10 : hexdigit + '0';
-}
-
-static int fmt_xlong(char *s, unsigned int i)
-{
-        char *bak = s;
-        *s = tohex((i >> 12) & 0xf);
-        if (s != bak || *s != '0')
-                ++s;
-        *s = tohex((i >> 8) & 0xf);
-        if (s != bak || *s != '0')
-                ++s;
-        *s = tohex((i >> 4) & 0xf);
-        if (s != bak || *s != '0')
-                ++s;
-        *s = tohex(i & 0xf);
-        return s - bak + 1;
-}
-
-static unsigned int fmt_ip6(char *s, const char ip[16])
-{
-        unsigned int len;
-        unsigned int i;
-        unsigned int temp;
-        unsigned int compressing;
-        int j;
-
-        len = 0;
-        compressing = 0;
-        for (j = 0; j < 16; j += 2) {
-
-#ifdef V4MAPPEDPREFIX
-                if (j == 12 && !memcmp(ip, V4mappedprefix, 12)) {
-                        inet_ntoa_r(*(struct in_addr *)(ip + 12), s);
-                        temp = strlen(s);
-                        return len + temp;
-                }
-#endif
-                temp = ((unsigned long)(unsigned char)ip[j] << 8) +
-                    (unsigned long)(unsigned char)ip[j + 1];
-                if (temp == 0) {
-                        if (!compressing) {
-                                compressing = 1;
-                                if (j == 0) {
-                                        *s++ = ':';
-                                        ++len;
-                                }
-                        }
-                } else {
-                        if (compressing) {
-                                compressing = 0;
-                                *s++ = ':';
-                                ++len;
-                        }
-                        i = fmt_xlong(s, temp);
-                        len += i;
-                        s += i;
-                        if (j < 14) {
-                                *s++ = ':';
-                                ++len;
-                        }
-                }
-        }
-        if (compressing) {
-                *s++ = ':';
-                ++len;
-        }
-        *s = 0;
-        return len;
-}
-
 static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
                                         struct pktgen_dev *pkt_dev)
 {
@@ -2959,7 +2860,6 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
         struct udphdr *udph;
         int datalen;
         struct ipv6hdr *iph;
-        struct pktgen_hdr *pgh = NULL;
         __be16 protocol = htons(ETH_P_IPV6);
         __be32 *mpls;
         __be16 *vlan_tci = NULL;                 /* Encapsulates priority and VLAN ID */
@@ -2977,8 +2877,8 @@ 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);
+        queue_map = pkt_dev->cur_queue_map;
 
         skb = __netdev_alloc_skb(odev,
                                  pkt_dev->cur_pkt_size + 64
@@ -2987,6 +2887,7 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
                 sprintf(pkt_dev->result, "No memory");
                 return NULL;
         }
+        prefetchw(skb->data);
 
         skb_reserve(skb, 16);
 
@@ -3017,6 +2918,7 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
         skb->transport_header = skb->network_header + sizeof(struct ipv6hdr);
         skb_put(skb, sizeof(struct ipv6hdr) + sizeof(struct udphdr));
         skb_set_queue_mapping(skb, queue_map);
+        skb->priority = pkt_dev->skb_priority;
         iph = ipv6_hdr(skb);
         udph = udp_hdr(skb);
 
@@ -3060,75 +2962,7 @@ static struct sk_buff *fill_packet_ipv6(struct net_device *odev,
         skb->dev = odev;
         skb->pkt_type = PACKET_HOST;
 
-        if (pkt_dev->nfrags <= 0)
-                pgh = (struct pktgen_hdr *)skb_put(skb, datalen);
-        else {
-                int frags = pkt_dev->nfrags;
-                int i;
-
-                pgh = (struct pktgen_hdr *)(((char *)(udph)) + 8);
-
-                if (frags > MAX_SKB_FRAGS)
-                        frags = MAX_SKB_FRAGS;
-                if (datalen > frags * PAGE_SIZE) {
-                        skb_put(skb, datalen - frags * PAGE_SIZE);
-                        datalen = frags * PAGE_SIZE;
-                }
-
-                i = 0;
-                while (datalen > 0) {
-                        struct page *page = alloc_pages(GFP_KERNEL, 0);
-                        skb_shinfo(skb)->frags[i].page = page;
-                        skb_shinfo(skb)->frags[i].page_offset = 0;
-                        skb_shinfo(skb)->frags[i].size =
-                            (datalen < PAGE_SIZE ? datalen : PAGE_SIZE);
-                        datalen -= skb_shinfo(skb)->frags[i].size;
-                        skb->len += skb_shinfo(skb)->frags[i].size;
-                        skb->data_len += skb_shinfo(skb)->frags[i].size;
-                        i++;
-                        skb_shinfo(skb)->nr_frags = i;
-                }
-
-                while (i < frags) {
-                        int rem;
-
-                        if (i == 0)
-                                break;
-
-                        rem = skb_shinfo(skb)->frags[i - 1].size / 2;
-                        if (rem == 0)
-                                break;
-
-                        skb_shinfo(skb)->frags[i - 1].size -= rem;
-
-                        skb_shinfo(skb)->frags[i] =
-                            skb_shinfo(skb)->frags[i - 1];
-                        get_page(skb_shinfo(skb)->frags[i].page);
-                        skb_shinfo(skb)->frags[i].page =
-                            skb_shinfo(skb)->frags[i - 1].page;
-                        skb_shinfo(skb)->frags[i].page_offset +=
-                            skb_shinfo(skb)->frags[i - 1].size;
-                        skb_shinfo(skb)->frags[i].size = rem;
-                        i++;
-                        skb_shinfo(skb)->nr_frags = i;
-                }
-        }
-
-        /* Stamp the time, and sequence number,
-         * convert them to network byte order
-         * should we update cloned packets too ?
-         */
-        if (pgh) {
-                struct timeval timestamp;
-
-                pgh->pgh_magic = htonl(PKTGEN_MAGIC);
-                pgh->seq_num = htonl(pkt_dev->seq_num);
-
-                do_gettimeofday(&timestamp);
-                pgh->tv_sec = htonl(timestamp.tv_sec);
-                pgh->tv_usec = htonl(timestamp.tv_usec);
-        }
-        /* pkt_dev->seq_num++; FF: you really mean this? */
+        pktgen_finalize_skb(pkt_dev, skb, datalen);
 
         return skb;
 }
@@ -3298,7 +3132,7 @@ static void show_results(struct pktgen_dev *pkt_dev, int nr_frags)
                                     pkt_dev->started_at);
         ktime_t idle = ns_to_ktime(pkt_dev->idle_acc);
 
-        p += sprintf(p, "OK: %llu(c%llu+d%llu) nsec, %llu (%dbyte,%dfrags)\n",
+        p += sprintf(p, "OK: %llu(c%llu+d%llu) usec, %llu (%dbyte,%dfrags)\n",
                      (unsigned long long)ktime_to_us(elapsed),
                      (unsigned long long)ktime_to_us(ktime_sub(elapsed, idle)),
                      (unsigned long long)ktime_to_us(idle),
@@ -3432,11 +3266,6 @@ static void pktgen_rem_thread(struct pktgen_thread *t)
 
         remove_proc_entry(t->tsk->comm, pg_proc_dir);
 
-        mutex_lock(&pktgen_thread_lock);
-
-        list_del(&t->th_list);
-
-        mutex_unlock(&pktgen_thread_lock);
 }
 
 static void pktgen_resched(struct pktgen_dev *pkt_dev)
@@ -3511,7 +3340,7 @@ static void pktgen_xmit(struct pktgen_dev *pkt_dev)
 
         __netif_tx_lock_bh(txq);
 
-        if (unlikely(netif_tx_queue_stopped(txq) || netif_tx_queue_frozen(txq))) {
+        if (unlikely(netif_tx_queue_frozen_or_stopped(txq))) {
                 ret = NETDEV_TX_BUSY;
                 pkt_dev->last_ok = 0;
                 goto unlock;
@@ -3535,8 +3364,7 @@ static void pktgen_xmit(struct pktgen_dev *pkt_dev)
                 break;
         default: /* Drivers are not supposed to return other values! */
                 if (net_ratelimit())
-                        pr_info("pktgen: %s xmit error: %d\n",
-                                pkt_dev->odevname, ret);
+                        pr_info("%s xmit error: %d\n", pkt_dev->odevname, ret);
                 pkt_dev->errors++;
                 /* fallthru */
         case NETDEV_TX_LOCKED:
@@ -3583,6 +3411,8 @@ static int pktgen_thread_worker(void *arg)
                 pkt_dev = next_to_run(t);
 
                 if (unlikely(!pkt_dev && t->control == 0)) {
+                        if (pktgen_exiting)
+                                break;
                         wait_event_interruptible_timeout(t->queue,
                                                          t->control != 0,
                                                          HZ/10);
@@ -3635,6 +3465,13 @@ static int pktgen_thread_worker(void *arg)
         pr_debug("%s removing thread\n", t->tsk->comm);
         pktgen_rem_thread(t);
 
+        /* Wait for kthread_stop */
+        while (!kthread_should_stop()) {
+                set_current_state(TASK_INTERRUPTIBLE);
+                schedule();
+        }
+        __set_current_state(TASK_RUNNING);
+
         return 0;
 }
 
@@ -3707,13 +3544,12 @@ static int pktgen_add_device(struct pktgen_thread *t, const char *ifname)
                 return -ENOMEM;
 
         strcpy(pkt_dev->odevname, ifname);
-        pkt_dev->flows = vmalloc_node(MAX_CFLOWS * sizeof(struct flow_state),
+        pkt_dev->flows = vzalloc_node(MAX_CFLOWS * sizeof(struct flow_state),
                                       node);
         if (pkt_dev->flows == NULL) {
                 kfree(pkt_dev);
                 return -ENOMEM;
         }
-        memset(pkt_dev->flows, 0, MAX_CFLOWS * sizeof(struct flow_state));
 
         pkt_dev->removal_mark = 0;
         pkt_dev->min_pkt_size = ETH_ZLEN;
@@ -3786,7 +3622,10 @@ static int __init pktgen_create_thread(int cpu)
         list_add_tail(&t->th_list, &pktgen_threads);
         init_completion(&t->start_done);
 
-        p = kthread_create(pktgen_thread_worker, t, "kpktgend_%d", cpu);
+        p = kthread_create_on_node(pktgen_thread_worker,
+                                   t,
+                                   cpu_to_node(cpu),
+                                   "kpktgend_%d", cpu);
         if (IS_ERR(p)) {
                 pr_err("kernel_thread() failed for cpu %d\n", t->cpu);
                 list_del(&t->th_list);
@@ -3858,6 +3697,8 @@ static int pktgen_remove_device(struct pktgen_thread *t,
         free_SAs(pkt_dev);
 #endif
         vfree(pkt_dev->flows);
+        if (pkt_dev->page)
+                put_page(pkt_dev->page);
         kfree(pkt_dev);
         return 0;
 }
@@ -3866,6 +3707,7 @@ static int __init pg_init(void)
 {
         int cpu;
         struct proc_dir_entry *pe;
+        int ret = 0;
 
         pr_info("%s", version);
 
@@ -3876,11 +3718,10 @@ static int __init pg_init(void)
         pe = proc_create(PGCTRL, 0600, pg_proc_dir, &pktgen_fops);
         if (pe == NULL) {
                 pr_err("ERROR: cannot create %s procfs entry\n", PGCTRL);
-                proc_net_remove(&init_net, PG_PROC_DIR);
-                return -EINVAL;
+                ret = -EINVAL;
+                goto remove_dir;
         }
 
-        /* Register us to receive netdevice events */
         register_netdevice_notifier(&pktgen_notifier_block);
 
         for_each_online_cpu(cpu) {
@@ -3894,23 +3735,27 @@ static int __init pg_init(void)
 
         if (list_empty(&pktgen_threads)) {
                 pr_err("ERROR: Initialization failed for all threads\n");
-                unregister_netdevice_notifier(&pktgen_notifier_block);
-                remove_proc_entry(PGCTRL, pg_proc_dir);
-                proc_net_remove(&init_net, PG_PROC_DIR);
-                return -ENODEV;
+                ret = -ENODEV;
+                goto unregister;
         }
 
         return 0;
+
+ unregister:
+        unregister_netdevice_notifier(&pktgen_notifier_block);
+        remove_proc_entry(PGCTRL, pg_proc_dir);
+ remove_dir:
+        proc_net_remove(&init_net, PG_PROC_DIR);
+        return ret;
 }
 
 static void __exit pg_cleanup(void)
 {
         struct pktgen_thread *t;
         struct list_head *q, *n;
-        wait_queue_head_t queue;
-        init_waitqueue_head(&queue);
 
         /* Stop all interfaces & threads */
+        pktgen_exiting = true;
 
         list_for_each_safe(q, n, &pktgen_threads) {
                 t = list_entry(q, struct pktgen_thread, th_list);