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);