1 files changed, 61 insertions, 6 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 3ddce538ef4a..7f442030f5ad 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -108,12 +108,15 @@ void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
 
 void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
 {
+        if (!skb)
+                return;
+
         rt2x00queue_unmap_skb(rt2x00dev, skb);
         dev_kfree_skb_any(skb);
 }
 
-void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
-                                      struct txentry_desc *txdesc)
+static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
+                                             struct txentry_desc *txdesc)
 {
         struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
         struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
@@ -237,10 +240,9 @@ void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
                         txdesc->signal |= 0x08;
         }
 }
-EXPORT_SYMBOL_GPL(rt2x00queue_create_tx_descriptor);
 
-void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
-                                     struct txentry_desc *txdesc)
+static void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
+                                            struct txentry_desc *txdesc)
 {
         struct data_queue *queue = entry->queue;
         struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
@@ -270,7 +272,6 @@ void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
             !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
                 rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, queue->qid);
 }
-EXPORT_SYMBOL_GPL(rt2x00queue_write_tx_descriptor);
 
 int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
 {
@@ -320,6 +321,60 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         return 0;
 }
 
+int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
+                              struct ieee80211_vif *vif)
+{
+        struct rt2x00_intf *intf = vif_to_intf(vif);
+        struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
+        __le32 desc[16];
+
+        if (unlikely(!intf->beacon))
+                return -ENOBUFS;
+
+        intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
+        if (!intf->beacon->skb)
+                return -ENOMEM;
+
+        /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
+
+        /*
+         * For the descriptor we use a local array from where the
+         * driver can move it to the correct location required for
+         * the hardware.
+         */
+        memset(desc, 0, sizeof(desc));
+
+        /*
+         * Fill in skb descriptor
+         */
+        skbdesc = get_skb_frame_desc(intf->beacon->skb);
+        memset(skbdesc, 0, sizeof(*skbdesc));
+        skbdesc->desc = desc;
+        skbdesc->desc_len = intf->beacon->queue->desc_size;
+        skbdesc->entry = intf->beacon;
+
+        /*
+         * Write TX descriptor into reserved room in front of the beacon.
+         */
+        rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
+
+        /*
+         * Send beacon to hardware.
+         * Also enable beacon generation, which might have been disabled
+         * by the driver during the config_beacon() callback function.
+         */
+        rt2x00dev->ops->lib->write_beacon(intf->beacon);
+        rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, QID_BEACON);
+
+        return 0;
+}
+
 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
                                          const enum data_queue_qid queue)
 {