wl1251: Prepare for idle mode support

[pandora-wifi.git] / drivers / net / wireless / wl12xx / wl1251_tx.c

diff --git a/drivers/net/wireless/wl12xx/wl1251_tx.c b/drivers/net/wireless/wl12xx/wl1251_tx.c
index a38ec19..fe07cf2 100644 (file)
--- a/drivers/net/wireless/wl12xx/wl1251_tx.c
+++ b/drivers/net/wireless/wl12xx/wl1251_tx.c
@@ -215,16 +215,30 @@ static int wl1251_tx_send_packet(struct wl1251 *wl, struct sk_buff *skb,
                wl1251_debug(DEBUG_TX, "skb offset %d", offset);
 
                /* check whether the current skb can be used */
-               if (!skb_cloned(skb) && (skb_tailroom(skb) >= offset)) {
-                       unsigned char *src = skb->data;
+               if (skb_cloned(skb) || (skb_tailroom(skb) < offset)) {
+                       struct sk_buff *newskb = skb_copy_expand(skb, 0, 3,
+                                                                GFP_KERNEL);
+
+                       if (unlikely(newskb == NULL)) {
+                               wl1251_error("Can't allocate skb!");
+                               return -EINVAL;
+                       }
+
+                       tx_hdr = (struct tx_double_buffer_desc *) newskb->data;
+
+                       dev_kfree_skb_any(skb);
+                       wl->tx_frames[tx_hdr->id] = skb = newskb;
+
+                       offset = (4 - (long)skb->data) & 0x03;
+                       wl1251_debug(DEBUG_TX, "new skb offset %d", offset);
+               }
 
-                       /* align the buffer on a 4-byte boundary */
+               /* align the buffer on a 4-byte boundary */
+               if (offset) {
+                       unsigned char *src = skb->data;
                        skb_reserve(skb, offset);
                        memmove(skb->data, src, skb->len);
                        tx_hdr = (struct tx_double_buffer_desc *) skb->data;
-               } else {
-                       wl1251_info("No handler, fixme!");
-                       return -EINVAL;
                }
        }
 
@@ -322,11 +336,6 @@ void wl1251_tx_work(struct work_struct *work)
 
                ret = wl1251_tx_frame(wl, skb);
                if (ret == -EBUSY) {
-                       /* firmware buffer is full, stop queues */
-                       wl1251_debug(DEBUG_TX, "tx_work: fw buffer full, "
-                                    "stop queues");
-                       ieee80211_stop_queues(wl->hw);
-                       wl->tx_queue_stopped = true;
                        skb_queue_head(&wl->tx_queue, skb);
                        goto out;
                } else if (ret < 0) {
@@ -375,7 +384,7 @@ static void wl1251_tx_packet_cb(struct wl1251 *wl,
 {
        struct ieee80211_tx_info *info;
        struct sk_buff *skb;
-       int hdrlen, ret;
+       int hdrlen;
        u8 *frame;
 
        skb = wl->tx_frames[result->id];
@@ -414,41 +423,14 @@ static void wl1251_tx_packet_cb(struct wl1251 *wl,
        ieee80211_tx_status(wl->hw, skb);
 
        wl->tx_frames[result->id] = NULL;
-
-       if (wl->tx_queue_stopped) {
-               wl1251_debug(DEBUG_TX, "cb: queue was stopped");
-
-               skb = skb_dequeue(&wl->tx_queue);
-
-               /* The skb can be NULL because tx_work might have been
-                  scheduled before the queue was stopped making the
-                  queue empty */
-
-               if (skb) {
-                       ret = wl1251_tx_frame(wl, skb);
-                       if (ret == -EBUSY) {
-                               /* firmware buffer is still full */
-                               wl1251_debug(DEBUG_TX, "cb: fw buffer "
-                                            "still full");
-                               skb_queue_head(&wl->tx_queue, skb);
-                               return;
-                       } else if (ret < 0) {
-                               dev_kfree_skb(skb);
-                               return;
-                       }
-               }
-
-               wl1251_debug(DEBUG_TX, "cb: waking queues");
-               ieee80211_wake_queues(wl->hw);
-               wl->tx_queue_stopped = false;
-       }
 }
 
 /* Called upon reception of a TX complete interrupt */
 void wl1251_tx_complete(struct wl1251 *wl)
 {
-       int i, result_index, num_complete = 0;
+       int i, result_index, num_complete = 0, queue_len;
        struct tx_result result[FW_TX_CMPLT_BLOCK_SIZE], *result_ptr;
+       unsigned long flags;
 
        if (unlikely(wl->state != WL1251_STATE_ON))
                return;
@@ -477,6 +459,24 @@ void wl1251_tx_complete(struct wl1251 *wl)
                }
        }
 
+       queue_len = skb_queue_len(&wl->tx_queue);
+
+       if ((num_complete > 0) && (queue_len > 0)) {
+               /* firmware buffer has space, reschedule tx_work */
+               wl1251_debug(DEBUG_TX, "tx_complete: reschedule tx_work");
+               ieee80211_queue_work(wl->hw, &wl->tx_work);
+       }
+
+       if (wl->tx_queue_stopped &&
+           queue_len <= WL1251_TX_QUEUE_LOW_WATERMARK) {
+               /* tx_queue has space, restart queues */
+               wl1251_debug(DEBUG_TX, "tx_complete: waking queues");
+               spin_lock_irqsave(&wl->wl_lock, flags);
+               ieee80211_wake_queues(wl->hw);
+               wl->tx_queue_stopped = false;
+               spin_unlock_irqrestore(&wl->wl_lock, flags);
+       }
+
        /* Every completed frame needs to be acknowledged */
        if (num_complete) {
                /*