qeth: Fix scatter-gather regression

This patch fixes a scatter-gather regression introduced with

commit 5640f768 net: use a per task frag allocator

Now the qeth driver can cope with bigger framents and split a fragment in
sub framents if required.

Signed-off-by: Frank Blaschka <blaschka@linux.vnet.ibm.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

3 files changed, 42 insertions, 8 deletions

diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index d87961d4c0de..8c0622399fcd 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -916,6 +916,7 @@ int qeth_send_control_data(struct qeth_card *, int, struct qeth_cmd_buffer *,
         void *reply_param);
 int qeth_get_priority_queue(struct qeth_card *, struct sk_buff *, int, int);
 int qeth_get_elements_no(struct qeth_card *, void *, struct sk_buff *, int);
+int qeth_get_elements_for_frags(struct sk_buff *);
 int qeth_do_send_packet_fast(struct qeth_card *, struct qeth_qdio_out_q *,
                         struct sk_buff *, struct qeth_hdr *, int, int, int);
 int qeth_do_send_packet(struct qeth_card *, struct qeth_qdio_out_q *,
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 0d8cdff81813..0d73a999983d 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -3679,6 +3679,25 @@ int qeth_get_priority_queue(struct qeth_card *card, struct sk_buff *skb,
 }
 EXPORT_SYMBOL_GPL(qeth_get_priority_queue);
 
+int qeth_get_elements_for_frags(struct sk_buff *skb)
+{
+        int cnt, length, e, elements = 0;
+        struct skb_frag_struct *frag;
+        char *data;
+
+        for (cnt = 0; cnt < skb_shinfo(skb)->nr_frags; cnt++) {
+                frag = &skb_shinfo(skb)->frags[cnt];
+                data = (char *)page_to_phys(skb_frag_page(frag)) +
+                        frag->page_offset;
+                length = frag->size;
+                e = PFN_UP((unsigned long)data + length - 1) -
+                        PFN_DOWN((unsigned long)data);
+                elements += e;
+        }
+        return elements;
+}
+EXPORT_SYMBOL_GPL(qeth_get_elements_for_frags);
+
 int qeth_get_elements_no(struct qeth_card *card, void *hdr,
                      struct sk_buff *skb, int elems)
 {
@@ -3686,7 +3705,8 @@ int qeth_get_elements_no(struct qeth_card *card, void *hdr,
         int elements_needed = PFN_UP((unsigned long)skb->data + dlen - 1) -
                 PFN_DOWN((unsigned long)skb->data);
 
-        elements_needed += skb_shinfo(skb)->nr_frags;
+        elements_needed += qeth_get_elements_for_frags(skb);
+
         if ((elements_needed + elems) > QETH_MAX_BUFFER_ELEMENTS(card)) {
                 QETH_DBF_MESSAGE(2, "Invalid size of IP packet "
                         "(Number=%d / Length=%d). Discarded.\n",
@@ -3771,12 +3791,23 @@ static inline void __qeth_fill_buffer(struct sk_buff *skb,
 
         for (cnt = 0; cnt < skb_shinfo(skb)->nr_frags; cnt++) {
                 frag = &skb_shinfo(skb)->frags[cnt];
-                buffer->element[element].addr = (char *)
-                        page_to_phys(skb_frag_page(frag))
-                        + frag->page_offset;
-                buffer->element[element].length = frag->size;
-                buffer->element[element].eflags = SBAL_EFLAGS_MIDDLE_FRAG;
-                element++;
+                data = (char *)page_to_phys(skb_frag_page(frag)) +
+                        frag->page_offset;
+                length = frag->size;
+                while (length > 0) {
+                        length_here = PAGE_SIZE -
+                                ((unsigned long) data % PAGE_SIZE);
+                        if (length < length_here)
+                                length_here = length;
+
+                        buffer->element[element].addr = data;
+                        buffer->element[element].length = length_here;
+                        buffer->element[element].eflags =
+                                SBAL_EFLAGS_MIDDLE_FRAG;
+                        length -= length_here;
+                        data += length_here;
+                        element++;
+                }
         }
 
         if (buffer->element[element - 1].eflags)
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index b6da6cec9c3e..8710337dab3e 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -2903,7 +2903,9 @@ static inline int qeth_l3_tso_elements(struct sk_buff *skb)
                 tcp_hdr(skb)->doff * 4;
         int tcpd_len = skb->len - (tcpd - (unsigned long)skb->data);
         int elements = PFN_UP(tcpd + tcpd_len - 1) - PFN_DOWN(tcpd);
-        elements += skb_shinfo(skb)->nr_frags;
+
+        elements += qeth_get_elements_for_frags(skb);
+
         return elements;
 }