1 files changed, 86 insertions, 40 deletions

diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 8477fb4de614..a6df4c96150f 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -811,7 +811,80 @@ static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
         }
 }
 
-/* Returns the number of WRBs used up by the skb */
+/* Grab a WRB header for xmit */
+static u16 be_tx_get_wrb_hdr(struct be_tx_obj *txo)
+{
+        u16 head = txo->q.head;
+
+        queue_head_inc(&txo->q);
+        return head;
+}
+
+/* Set up the WRB header for xmit */
+static void be_tx_setup_wrb_hdr(struct be_adapter *adapter,
+                                struct be_tx_obj *txo,
+                                struct be_wrb_params *wrb_params,
+                                struct sk_buff *skb, u16 head)
+{
+        u32 num_frags = skb_wrb_cnt(skb);
+        struct be_queue_info *txq = &txo->q;
+        struct be_eth_hdr_wrb *hdr = queue_index_node(txq, head);
+
+        wrb_fill_hdr(adapter, hdr, wrb_params, skb);
+        be_dws_cpu_to_le(hdr, sizeof(*hdr));
+
+        BUG_ON(txo->sent_skb_list[head]);
+        txo->sent_skb_list[head] = skb;
+        txo->last_req_hdr = head;
+        atomic_add(num_frags, &txq->used);
+        txo->last_req_wrb_cnt = num_frags;
+        txo->pend_wrb_cnt += num_frags;
+}
+
+/* Setup a WRB fragment (buffer descriptor) for xmit */
+static void be_tx_setup_wrb_frag(struct be_tx_obj *txo, dma_addr_t busaddr,
+                                 int len)
+{
+        struct be_eth_wrb *wrb;
+        struct be_queue_info *txq = &txo->q;
+
+        wrb = queue_head_node(txq);
+        wrb_fill(wrb, busaddr, len);
+        queue_head_inc(txq);
+}
+
+/* Bring the queue back to the state it was in before be_xmit_enqueue() routine
+ * was invoked. The producer index is restored to the previous packet and the
+ * WRBs of the current packet are unmapped. Invoked to handle tx setup errors.
+ */
+static void be_xmit_restore(struct be_adapter *adapter,
+                            struct be_tx_obj *txo, u16 head, bool map_single,
+                            u32 copied)
+{
+        struct device *dev;
+        struct be_eth_wrb *wrb;
+        struct be_queue_info *txq = &txo->q;
+
+        dev = &adapter->pdev->dev;
+        txq->head = head;
+
+        /* skip the first wrb (hdr); it's not mapped */
+        queue_head_inc(txq);
+        while (copied) {
+                wrb = queue_head_node(txq);
+                unmap_tx_frag(dev, wrb, map_single);
+                map_single = false;
+                copied -= le32_to_cpu(wrb->frag_len);
+                queue_head_inc(txq);
+        }
+
+        txq->head = head;
+}
+
+/* Enqueue the given packet for transmit. This routine allocates WRBs for the
+ * packet, dma maps the packet buffers and sets up the WRBs. Returns the number
+ * of WRBs used up by the packet.
+ */
 static u32 be_xmit_enqueue(struct be_adapter *adapter, struct be_tx_obj *txo,
                            struct sk_buff *skb,
                            struct be_wrb_params *wrb_params)
@@ -819,70 +892,43 @@ static u32 be_xmit_enqueue(struct be_adapter *adapter, struct be_tx_obj *txo,
         u32 i, copied = 0, wrb_cnt = skb_wrb_cnt(skb);
         struct device *dev = &adapter->pdev->dev;
         struct be_queue_info *txq = &txo->q;
-        struct be_eth_hdr_wrb *hdr;
         bool map_single = false;
-        struct be_eth_wrb *wrb;
-        dma_addr_t busaddr;
         u16 head = txq->head;
+        dma_addr_t busaddr;
+        int len;
 
-        hdr = queue_head_node(txq);
-        wrb_fill_hdr(adapter, hdr, wrb_params, skb);
-        be_dws_cpu_to_le(hdr, sizeof(*hdr));
-
-        queue_head_inc(txq);
+        head = be_tx_get_wrb_hdr(txo);
 
         if (skb->len > skb->data_len) {
-                int len = skb_headlen(skb);
+                len = skb_headlen(skb);
 
                 busaddr = dma_map_single(dev, skb->data, len, DMA_TO_DEVICE);
                 if (dma_mapping_error(dev, busaddr))
                         goto dma_err;
                 map_single = true;
-                wrb = queue_head_node(txq);
-                wrb_fill(wrb, busaddr, len);
-                queue_head_inc(txq);
+                be_tx_setup_wrb_frag(txo, busaddr, len);
                 copied += len;
         }
 
         for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
                 const struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+                len = skb_frag_size(frag);
 
-                busaddr = skb_frag_dma_map(dev, frag, 0,
-                                           skb_frag_size(frag), DMA_TO_DEVICE);
+                busaddr = skb_frag_dma_map(dev, frag, 0, len, DMA_TO_DEVICE);
                 if (dma_mapping_error(dev, busaddr))
                         goto dma_err;
-                wrb = queue_head_node(txq);
-                wrb_fill(wrb, busaddr, skb_frag_size(frag));
-                queue_head_inc(txq);
-                copied += skb_frag_size(frag);
+                be_tx_setup_wrb_frag(txo, busaddr, len);
+                copied += len;
         }
 
-        BUG_ON(txo->sent_skb_list[head]);
-        txo->sent_skb_list[head] = skb;
-        txo->last_req_hdr = head;
-        atomic_add(wrb_cnt, &txq->used);
-        txo->last_req_wrb_cnt = wrb_cnt;
-        txo->pend_wrb_cnt += wrb_cnt;
+        be_tx_setup_wrb_hdr(adapter, txo, wrb_params, skb, head);
 
         be_tx_stats_update(txo, skb);
         return wrb_cnt;
 
 dma_err:
-        /* Bring the queue back to the state it was in before this
-         * routine was invoked.
-         */
-        txq->head = head;
-        /* skip the first wrb (hdr); it's not mapped */
-        queue_head_inc(txq);
-        while (copied) {
-                wrb = queue_head_node(txq);
-                unmap_tx_frag(dev, wrb, map_single);
-                map_single = false;
-                copied -= le32_to_cpu(wrb->frag_len);
-                adapter->drv_stats.dma_map_errors++;
-                queue_head_inc(txq);
-        }
-        txq->head = head;
+        adapter->drv_stats.dma_map_errors++;
+        be_xmit_restore(adapter, txo, head, map_single, copied);
         return 0;
 }