1 files changed, 39 insertions, 11 deletions

diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index a8b069cd9a4c..b3cee032f578 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -243,8 +243,7 @@ static void qeth_l2_get_packet_type(struct qeth_card *card,
 static void qeth_l2_fill_header(struct qeth_card *card, struct qeth_hdr *hdr,
                         struct sk_buff *skb, int ipv, int cast_type)
 {
-        struct vlan_ethhdr *veth = (struct vlan_ethhdr *)((skb->data) +
-                                        QETH_HEADER_SIZE);
+        struct vlan_ethhdr *veth = (struct vlan_ethhdr *)skb_mac_header(skb);
 
         memset(hdr, 0, sizeof(struct qeth_hdr));
         hdr->hdr.l2.id = QETH_HEADER_TYPE_LAYER2;
@@ -621,6 +620,9 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
         int tx_bytes = skb->len;
         enum qeth_large_send_types large_send = QETH_LARGE_SEND_NO;
         struct qeth_eddp_context *ctx = NULL;
+        int data_offset = -1;
+        int elements_needed = 0;
+        int hd_len = 0;
 
         if ((card->state != CARD_STATE_UP) || !card->lan_online) {
                 card->stats.tx_carrier_errors++;
@@ -643,13 +645,32 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
         if (card->info.type == QETH_CARD_TYPE_OSN)
                 hdr = (struct qeth_hdr *)skb->data;
         else {
-                /* create a clone with writeable headroom */
-                new_skb = skb_realloc_headroom(skb, sizeof(struct qeth_hdr));
-                if (!new_skb)
-                        goto tx_drop;
-                hdr = (struct qeth_hdr *)skb_push(new_skb,
+                if ((card->info.type == QETH_CARD_TYPE_IQD) && (!large_send) &&
+                    (skb_shinfo(skb)->nr_frags == 0)) {
+                        new_skb = skb;
+                        data_offset = ETH_HLEN;
+                        hd_len = ETH_HLEN;
+                        hdr = kmem_cache_alloc(qeth_core_header_cache,
+                                                GFP_ATOMIC);
+                        if (!hdr)
+                                goto tx_drop;
+                        elements_needed++;
+                        skb_reset_mac_header(new_skb);
+                        qeth_l2_fill_header(card, hdr, new_skb, ipv, cast_type);
+                        hdr->hdr.l2.pkt_length = new_skb->len;
+                        memcpy(((char *)hdr) + sizeof(struct qeth_hdr),
+                                skb_mac_header(new_skb), ETH_HLEN);
+                } else {
+                        /* create a clone with writeable headroom */
+                        new_skb = skb_realloc_headroom(skb,
+                                                sizeof(struct qeth_hdr));
+                        if (!new_skb)
+                                goto tx_drop;
+                        hdr = (struct qeth_hdr *)skb_push(new_skb,
                                                 sizeof(struct qeth_hdr));
-                qeth_l2_fill_header(card, hdr, new_skb, ipv, cast_type);
+                        skb_set_mac_header(new_skb, sizeof(struct qeth_hdr));
+                        qeth_l2_fill_header(card, hdr, new_skb, ipv, cast_type);
+                }
         }
 
         if (large_send == QETH_LARGE_SEND_EDDP) {
@@ -660,9 +681,13 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
                         goto tx_drop;
                 }
         } else {
-                elements = qeth_get_elements_no(card, (void *)hdr, new_skb, 0);
-                if (!elements)
+                elements = qeth_get_elements_no(card, (void *)hdr, new_skb,
+                                                elements_needed);
+                if (!elements) {
+                        if (data_offset >= 0)
+                                kmem_cache_free(qeth_core_header_cache, hdr);
                         goto tx_drop;
+                }
         }
 
         if ((large_send == QETH_LARGE_SEND_NO) &&
@@ -674,7 +699,7 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
                                          elements, ctx);
         else
                 rc = qeth_do_send_packet_fast(card, queue, new_skb, hdr,
-                                              elements, ctx);
+                                        elements, ctx, data_offset, hd_len);
         if (!rc) {
                 card->stats.tx_packets++;
                 card->stats.tx_bytes += tx_bytes;
@@ -701,6 +726,9 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
                 if (ctx != NULL)
                         qeth_eddp_put_context(ctx);
 
+                if (data_offset >= 0)
+                        kmem_cache_free(qeth_core_header_cache, hdr);
+
                 if (rc == -EBUSY) {
                         if (new_skb != skb)
                                 dev_kfree_skb_any(new_skb);