1 files changed, 184 insertions, 91 deletions

diff --git a/drivers/scsi/cxgb3i/cxgb3i_pdu.c b/drivers/scsi/cxgb3i/cxgb3i_pdu.c
index ce7ce8c6094c..17115c230d65 100644
--- a/drivers/scsi/cxgb3i/cxgb3i_pdu.c
+++ b/drivers/scsi/cxgb3i/cxgb3i_pdu.c
@@ -32,6 +32,10 @@
 #define cxgb3i_tx_debug(fmt...)
 #endif
 
+/* always allocate rooms for AHS */
+#define SKB_TX_PDU_HEADER_LEN   \
+        (sizeof(struct iscsi_hdr) + ISCSI_MAX_AHS_SIZE)
+static unsigned int skb_extra_headroom;
 static struct page *pad_page;
 
 /*
@@ -146,12 +150,13 @@ static inline void tx_skb_setmode(struct sk_buff *skb, int hcrc, int dcrc)
 
 void cxgb3i_conn_cleanup_task(struct iscsi_task *task)
 {
-        struct iscsi_tcp_task *tcp_task = task->dd_data;
+        struct cxgb3i_task_data *tdata = task->dd_data +
+                                        sizeof(struct iscsi_tcp_task);
 
         /* never reached the xmit task callout */
-        if (tcp_task->dd_data)
-                kfree_skb(tcp_task->dd_data);
-        tcp_task->dd_data = NULL;
+        if (tdata->skb)
+                __kfree_skb(tdata->skb);
+        memset(tdata, 0, sizeof(struct cxgb3i_task_data));
 
         /* MNC - Do we need a check in case this is called but
          * cxgb3i_conn_alloc_pdu has never been called on the task */
@@ -159,28 +164,102 @@ void cxgb3i_conn_cleanup_task(struct iscsi_task *task)
         iscsi_tcp_cleanup_task(task);
 }
 
-/*
- * We do not support ahs yet
- */
+static int sgl_seek_offset(struct scatterlist *sgl, unsigned int sgcnt,
+                                unsigned int offset, unsigned int *off,
+                                struct scatterlist **sgp)
+{
+        int i;
+        struct scatterlist *sg;
+
+        for_each_sg(sgl, sg, sgcnt, i) {
+                if (offset < sg->length) {
+                        *off = offset;
+                        *sgp = sg;
+                        return 0;
+                }
+                offset -= sg->length;
+        }
+        return -EFAULT;
+}
+
+static int sgl_read_to_frags(struct scatterlist *sg, unsigned int sgoffset,
+                                unsigned int dlen, skb_frag_t *frags,
+                                int frag_max)
+{
+        unsigned int datalen = dlen;
+        unsigned int sglen = sg->length - sgoffset;
+        struct page *page = sg_page(sg);
+        int i;
+
+        i = 0;
+        do {
+                unsigned int copy;
+
+                if (!sglen) {
+                        sg = sg_next(sg);
+                        if (!sg) {
+                                cxgb3i_log_error("%s, sg NULL, len %u/%u.\n",
+                                                 __func__, datalen, dlen);
+                                return -EINVAL;
+                        }
+                        sgoffset = 0;
+                        sglen = sg->length;
+                        page = sg_page(sg);
+
+                }
+                copy = min(datalen, sglen);
+                if (i && page == frags[i - 1].page &&
+                    sgoffset + sg->offset ==
+                        frags[i - 1].page_offset + frags[i - 1].size) {
+                        frags[i - 1].size += copy;
+                } else {
+                        if (i >= frag_max) {
+                                cxgb3i_log_error("%s, too many pages %u, "
+                                                 "dlen %u.\n", __func__,
+                                                 frag_max, dlen);
+                                return -EINVAL;
+                        }
+
+                        frags[i].page = page;
+                        frags[i].page_offset = sg->offset + sgoffset;
+                        frags[i].size = copy;
+                        i++;
+                }
+                datalen -= copy;
+                sgoffset += copy;
+                sglen -= copy;
+        } while (datalen);
+
+        return i;
+}
+
 int cxgb3i_conn_alloc_pdu(struct iscsi_task *task, u8 opcode)
 {
+        struct iscsi_conn *conn = task->conn;
         struct iscsi_tcp_task *tcp_task = task->dd_data;
-        struct sk_buff *skb;
+        struct cxgb3i_task_data *tdata = task->dd_data + sizeof(*tcp_task);
+        struct scsi_cmnd *sc = task->sc;
+        int headroom = SKB_TX_PDU_HEADER_LEN;
 
+        tcp_task->dd_data = tdata;
         task->hdr = NULL;
-        /* always allocate rooms for AHS */
-        skb = alloc_skb(sizeof(struct iscsi_hdr) + ISCSI_MAX_AHS_SIZE +
-                        TX_HEADER_LEN,  GFP_ATOMIC);
-        if (!skb)
+
+        /* write command, need to send data pdus */
+        if (skb_extra_headroom && (opcode == ISCSI_OP_SCSI_DATA_OUT ||
+            (opcode == ISCSI_OP_SCSI_CMD &&
+            (scsi_bidi_cmnd(sc) || sc->sc_data_direction == DMA_TO_DEVICE))))
+                headroom += min(skb_extra_headroom, conn->max_xmit_dlength);
+
+        tdata->skb = alloc_skb(TX_HEADER_LEN + headroom, GFP_ATOMIC);
+        if (!tdata->skb)
                 return -ENOMEM;
+        skb_reserve(tdata->skb, TX_HEADER_LEN);
 
         cxgb3i_tx_debug("task 0x%p, opcode 0x%x, skb 0x%p.\n",
-                        task, opcode, skb);
+                        task, opcode, tdata->skb);
 
-        tcp_task->dd_data = skb;
-        skb_reserve(skb, TX_HEADER_LEN);
-        task->hdr = (struct iscsi_hdr *)skb->data;
-        task->hdr_max = sizeof(struct iscsi_hdr);
+        task->hdr = (struct iscsi_hdr *)tdata->skb->data;
+        task->hdr_max = SKB_TX_PDU_HEADER_LEN;
 
         /* data_out uses scsi_cmd's itt */
         if (opcode != ISCSI_OP_SCSI_DATA_OUT)
@@ -192,13 +271,13 @@ int cxgb3i_conn_alloc_pdu(struct iscsi_task *task, u8 opcode)
 int cxgb3i_conn_init_pdu(struct iscsi_task *task, unsigned int offset,
                               unsigned int count)
 {
-        struct iscsi_tcp_task *tcp_task = task->dd_data;
-        struct sk_buff *skb = tcp_task->dd_data;
         struct iscsi_conn *conn = task->conn;
-        struct page *pg;
+        struct iscsi_tcp_task *tcp_task = task->dd_data;
+        struct cxgb3i_task_data *tdata = tcp_task->dd_data;
+        struct sk_buff *skb = tdata->skb;
         unsigned int datalen = count;
         int i, padlen = iscsi_padding(count);
-        skb_frag_t *frag;
+        struct page *pg;
 
         cxgb3i_tx_debug("task 0x%p,0x%p, offset %u, count %u, skb 0x%p.\n",
                         task, task->sc, offset, count, skb);
@@ -209,90 +288,94 @@ int cxgb3i_conn_init_pdu(struct iscsi_task *task, unsigned int offset,
                 return 0;
 
         if (task->sc) {
-                struct scatterlist *sg;
-                struct scsi_data_buffer *sdb;
-                unsigned int sgoffset = offset;
-                struct page *sgpg;
-                unsigned int sglen;
-
-                sdb = scsi_out(task->sc);
-                sg = sdb->table.sgl;
-
-                for_each_sg(sdb->table.sgl, sg, sdb->table.nents, i) {
-                        cxgb3i_tx_debug("sg %d, page 0x%p, len %u offset %u\n",
-                                        i, sg_page(sg), sg->length, sg->offset);
-
-                        if (sgoffset < sg->length)
-                                break;
-                        sgoffset -= sg->length;
+                struct scsi_data_buffer *sdb = scsi_out(task->sc);
+                struct scatterlist *sg = NULL;
+                int err;
+
+                tdata->offset = offset;
+                tdata->count = count;
+                err = sgl_seek_offset(sdb->table.sgl, sdb->table.nents,
+                                        tdata->offset, &tdata->sgoffset, &sg);
+                if (err < 0) {
+                        cxgb3i_log_warn("tpdu, sgl %u, bad offset %u/%u.\n",
+                                        sdb->table.nents, tdata->offset,
+                                        sdb->length);
+                        return err;
                 }
-                sgpg = sg_page(sg);
-                sglen = sg->length - sgoffset;
-
-                do {
-                        int j = skb_shinfo(skb)->nr_frags;
-                        unsigned int copy;
-
-                        if (!sglen) {
-                                sg = sg_next(sg);
-                                sgpg = sg_page(sg);
-                                sgoffset = 0;
-                                sglen = sg->length;
-                                ++i;
+                err = sgl_read_to_frags(sg, tdata->sgoffset, tdata->count,
+                                        tdata->frags, MAX_PDU_FRAGS);
+                if (err < 0) {
+                        cxgb3i_log_warn("tpdu, sgl %u, bad offset %u + %u.\n",
+                                        sdb->table.nents, tdata->offset,
+                                        tdata->count);
+                        return err;
+                }
+                tdata->nr_frags = err;
+
+                if (tdata->nr_frags > MAX_SKB_FRAGS ||
+                    (padlen && tdata->nr_frags == MAX_SKB_FRAGS)) {
+                        char *dst = skb->data + task->hdr_len;
+                        skb_frag_t *frag = tdata->frags;
+
+                        /* data fits in the skb's headroom */
+                        for (i = 0; i < tdata->nr_frags; i++, frag++) {
+                                char *src = kmap_atomic(frag->page,
+                                                        KM_SOFTIRQ0);
+
+                                memcpy(dst, src+frag->page_offset, frag->size);
+                                dst += frag->size;
+                                kunmap_atomic(src, KM_SOFTIRQ0);
                         }
-                        copy = min(sglen, datalen);
-                        if (j && skb_can_coalesce(skb, j, sgpg,
-                                                  sg->offset + sgoffset)) {
-                                skb_shinfo(skb)->frags[j - 1].size += copy;
-                        } else {
-                                get_page(sgpg);
-                                skb_fill_page_desc(skb, j, sgpg,
-                                                   sg->offset + sgoffset, copy);
+                        if (padlen) {
+                                memset(dst, 0, padlen);
+                                padlen = 0;
                         }
-                        sgoffset += copy;
-                        sglen -= copy;
-                        datalen -= copy;
-                } while (datalen);
+                        skb_put(skb, count + padlen);
+                } else {
+                        /* data fit into frag_list */
+                        for (i = 0; i < tdata->nr_frags; i++)
+                                get_page(tdata->frags[i].page);
+
+                        memcpy(skb_shinfo(skb)->frags, tdata->frags,
+                                sizeof(skb_frag_t) * tdata->nr_frags);
+                        skb_shinfo(skb)->nr_frags = tdata->nr_frags;
+                        skb->len += count;
+                        skb->data_len += count;
+                        skb->truesize += count;
+                }
+
         } else {
                 pg = virt_to_page(task->data);
 
-                while (datalen) {
-                        i = skb_shinfo(skb)->nr_frags;
-                        frag = &skb_shinfo(skb)->frags[i];
-
-                        get_page(pg);
-                        frag->page = pg;
-                        frag->page_offset = 0;
-                        frag->size = min((unsigned int)PAGE_SIZE, datalen);
-
-                        skb_shinfo(skb)->nr_frags++;
-                        datalen -= frag->size;
-                        pg++;
-                }
+                get_page(pg);
+                skb_fill_page_desc(skb, 0, pg, offset_in_page(task->data),
+                                        count);
+                skb->len += count;
+                skb->data_len += count;
+                skb->truesize += count;
         }
 
         if (padlen) {
                 i = skb_shinfo(skb)->nr_frags;
-                frag = &skb_shinfo(skb)->frags[i];
-                frag->page = pad_page;
-                frag->page_offset = 0;
-                frag->size = padlen;
-                skb_shinfo(skb)->nr_frags++;
+                get_page(pad_page);
+                skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags, pad_page, 0,
+                                 padlen);
+
+                skb->data_len += padlen;
+                skb->truesize += padlen;
+                skb->len += padlen;
         }
 
-        datalen = count + padlen;
-        skb->data_len += datalen;
-        skb->truesize += datalen;
-        skb->len += datalen;
         return 0;
 }
 
 int cxgb3i_conn_xmit_pdu(struct iscsi_task *task)
 {
-        struct iscsi_tcp_task *tcp_task = task->dd_data;
-        struct sk_buff *skb = tcp_task->dd_data;
         struct iscsi_tcp_conn *tcp_conn = task->conn->dd_data;
         struct cxgb3i_conn *cconn = tcp_conn->dd_data;
+        struct iscsi_tcp_task *tcp_task = task->dd_data;
+        struct cxgb3i_task_data *tdata = tcp_task->dd_data;
+        struct sk_buff *skb = tdata->skb;
         unsigned int datalen;
         int err;
 
@@ -300,13 +383,14 @@ int cxgb3i_conn_xmit_pdu(struct iscsi_task *task)
                 return 0;
 
         datalen = skb->data_len;
-        tcp_task->dd_data = NULL;
+        tdata->skb = NULL;
         err = cxgb3i_c3cn_send_pdus(cconn->cep->c3cn, skb);
-        cxgb3i_tx_debug("task 0x%p, skb 0x%p, len %u/%u, rv %d.\n",
-                        task, skb, skb->len, skb->data_len, err);
         if (err > 0) {
                 int pdulen = err;
 
+        cxgb3i_tx_debug("task 0x%p, skb 0x%p, len %u/%u, rv %d.\n",
+                        task, skb, skb->len, skb->data_len, err);
+
                 if (task->conn->hdrdgst_en)
                         pdulen += ISCSI_DIGEST_SIZE;
                 if (datalen && task->conn->datadgst_en)
@@ -325,12 +409,14 @@ int cxgb3i_conn_xmit_pdu(struct iscsi_task *task)
                 return err;
         }
         /* reset skb to send when we are called again */
-        tcp_task->dd_data = skb;
+        tdata->skb = skb;
         return -EAGAIN;
 }
 
 int cxgb3i_pdu_init(void)
 {
+        if (SKB_TX_HEADROOM > (512 * MAX_SKB_FRAGS))
+                skb_extra_headroom = SKB_TX_HEADROOM;
         pad_page = alloc_page(GFP_KERNEL);
         if (!pad_page)
                 return -ENOMEM;
@@ -366,7 +452,9 @@ void cxgb3i_conn_pdu_ready(struct s3_conn *c3cn)
         skb = skb_peek(&c3cn->receive_queue);
         while (!err && skb) {
                 __skb_unlink(skb, &c3cn->receive_queue);
-                read += skb_ulp_pdulen(skb);
+                read += skb_rx_pdulen(skb);
+                cxgb3i_rx_debug("conn 0x%p, cn 0x%p, rx skb 0x%p, pdulen %u.\n",
+                                conn, c3cn, skb, skb_rx_pdulen(skb));
                 err = cxgb3i_conn_read_pdu_skb(conn, skb);
                 __kfree_skb(skb);
                 skb = skb_peek(&c3cn->receive_queue);
@@ -377,6 +465,11 @@ void cxgb3i_conn_pdu_ready(struct s3_conn *c3cn)
                 cxgb3i_c3cn_rx_credits(c3cn, read);
         }
         conn->rxdata_octets += read;
+
+        if (err) {
+                cxgb3i_log_info("conn 0x%p rx failed err %d.\n", conn, err);
+                iscsi_conn_failure(conn, ISCSI_ERR_CONN_FAILED);
+        }
 }
 
 void cxgb3i_conn_tx_open(struct s3_conn *c3cn)