1
2
3
4
5
6
7
8
9
10
11
12
13
14
15#include <linux/slab.h>
16#include <linux/kernel.h>
17#include <linux/module.h>
18#include <linux/dma-mapping.h>
19
20#include "rt2x00.h"
21#include "rt2x00lib.h"
22
23struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry, gfp_t gfp)
24{
25 struct data_queue *queue = entry->queue;
26 struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
27 struct sk_buff *skb;
28 struct skb_frame_desc *skbdesc;
29 unsigned int frame_size;
30 unsigned int head_size = 0;
31 unsigned int tail_size = 0;
32
33
34
35
36
37 frame_size = queue->data_size + queue->desc_size + queue->winfo_size;
38
39
40
41
42
43
44 head_size = 4;
45
46
47
48
49
50
51 if (rt2x00_has_cap_hw_crypto(rt2x00dev)) {
52 head_size += 8;
53 tail_size += 8;
54 }
55
56
57
58
59 skb = __dev_alloc_skb(frame_size + head_size + tail_size, gfp);
60 if (!skb)
61 return NULL;
62
63
64
65
66
67 skb_reserve(skb, head_size);
68 skb_put(skb, frame_size);
69
70
71
72
73 skbdesc = get_skb_frame_desc(skb);
74 memset(skbdesc, 0, sizeof(*skbdesc));
75
76 if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DMA)) {
77 dma_addr_t skb_dma;
78
79 skb_dma = dma_map_single(rt2x00dev->dev, skb->data, skb->len,
80 DMA_FROM_DEVICE);
81 if (unlikely(dma_mapping_error(rt2x00dev->dev, skb_dma))) {
82 dev_kfree_skb_any(skb);
83 return NULL;
84 }
85
86 skbdesc->skb_dma = skb_dma;
87 skbdesc->flags |= SKBDESC_DMA_MAPPED_RX;
88 }
89
90 return skb;
91}
92
93int rt2x00queue_map_txskb(struct queue_entry *entry)
94{
95 struct device *dev = entry->queue->rt2x00dev->dev;
96 struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
97
98 skbdesc->skb_dma =
99 dma_map_single(dev, entry->skb->data, entry->skb->len, DMA_TO_DEVICE);
100
101 if (unlikely(dma_mapping_error(dev, skbdesc->skb_dma)))
102 return -ENOMEM;
103
104 skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
105 rt2x00lib_dmadone(entry);
106 return 0;
107}
108EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);
109
110void rt2x00queue_unmap_skb(struct queue_entry *entry)
111{
112 struct device *dev = entry->queue->rt2x00dev->dev;
113 struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
114
115 if (skbdesc->flags & SKBDESC_DMA_MAPPED_RX) {
116 dma_unmap_single(dev, skbdesc->skb_dma, entry->skb->len,
117 DMA_FROM_DEVICE);
118 skbdesc->flags &= ~SKBDESC_DMA_MAPPED_RX;
119 } else if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
120 dma_unmap_single(dev, skbdesc->skb_dma, entry->skb->len,
121 DMA_TO_DEVICE);
122 skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
123 }
124}
125EXPORT_SYMBOL_GPL(rt2x00queue_unmap_skb);
126
127void rt2x00queue_free_skb(struct queue_entry *entry)
128{
129 if (!entry->skb)
130 return;
131
132 rt2x00queue_unmap_skb(entry);
133 dev_kfree_skb_any(entry->skb);
134 entry->skb = NULL;
135}
136
137void rt2x00queue_align_frame(struct sk_buff *skb)
138{
139 unsigned int frame_length = skb->len;
140 unsigned int align = ALIGN_SIZE(skb, 0);
141
142 if (!align)
143 return;
144
145 skb_push(skb, align);
146 memmove(skb->data, skb->data + align, frame_length);
147 skb_trim(skb, frame_length);
148}
149
150
151
152
153
154void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int hdr_len)
155{
156 unsigned int l2pad = (skb->len > hdr_len) ? L2PAD_SIZE(hdr_len) : 0;
157
158 if (!l2pad)
159 return;
160
161 skb_push(skb, l2pad);
162 memmove(skb->data, skb->data + l2pad, hdr_len);
163}
164
165void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int hdr_len)
166{
167 unsigned int l2pad = (skb->len > hdr_len) ? L2PAD_SIZE(hdr_len) : 0;
168
169 if (!l2pad)
170 return;
171
172 memmove(skb->data + l2pad, skb->data, hdr_len);
173 skb_pull(skb, l2pad);
174}
175
176static void rt2x00queue_create_tx_descriptor_seq(struct rt2x00_dev *rt2x00dev,
177 struct sk_buff *skb,
178 struct txentry_desc *txdesc)
179{
180 struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
181 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
182 struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
183 u16 seqno;
184
185 if (!(tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ))
186 return;
187
188 __set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
189
190 if (!rt2x00_has_cap_flag(rt2x00dev, REQUIRE_SW_SEQNO)) {
191
192
193
194
195
196
197
198 if (ieee80211_is_beacon(hdr->frame_control)) {
199 __set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
200
201 return;
202 }
203
204 __clear_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
205 }
206
207
208
209
210
211
212
213
214
215
216
217
218 if (test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags))
219 seqno = atomic_add_return(0x10, &intf->seqno);
220 else
221 seqno = atomic_read(&intf->seqno);
222
223 hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
224 hdr->seq_ctrl |= cpu_to_le16(seqno);
225}
226
227static void rt2x00queue_create_tx_descriptor_plcp(struct rt2x00_dev *rt2x00dev,
228 struct sk_buff *skb,
229 struct txentry_desc *txdesc,
230 const struct rt2x00_rate *hwrate)
231{
232 struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
233 struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
234 unsigned int data_length;
235 unsigned int duration;
236 unsigned int residual;
237
238
239
240
241
242
243 if (test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags))
244 txdesc->u.plcp.ifs = IFS_BACKOFF;
245 else
246 txdesc->u.plcp.ifs = IFS_SIFS;
247
248
249 data_length = skb->len + 4;
250 data_length += rt2x00crypto_tx_overhead(rt2x00dev, skb);
251
252
253
254
255
256 txdesc->u.plcp.signal = hwrate->plcp;
257 txdesc->u.plcp.service = 0x04;
258
259 if (hwrate->flags & DEV_RATE_OFDM) {
260 txdesc->u.plcp.length_high = (data_length >> 6) & 0x3f;
261 txdesc->u.plcp.length_low = data_length & 0x3f;
262 } else {
263
264
265
266 residual = GET_DURATION_RES(data_length, hwrate->bitrate);
267 duration = GET_DURATION(data_length, hwrate->bitrate);
268
269 if (residual != 0) {
270 duration++;
271
272
273
274
275 if (hwrate->bitrate == 110 && residual <= 30)
276 txdesc->u.plcp.service |= 0x80;
277 }
278
279 txdesc->u.plcp.length_high = (duration >> 8) & 0xff;
280 txdesc->u.plcp.length_low = duration & 0xff;
281
282
283
284
285
286 if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
287 txdesc->u.plcp.signal |= 0x08;
288 }
289}
290
291static void rt2x00queue_create_tx_descriptor_ht(struct rt2x00_dev *rt2x00dev,
292 struct sk_buff *skb,
293 struct txentry_desc *txdesc,
294 struct ieee80211_sta *sta,
295 const struct rt2x00_rate *hwrate)
296{
297 struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
298 struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
299 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
300 struct rt2x00_sta *sta_priv = NULL;
301 u8 density = 0;
302
303 if (sta) {
304 sta_priv = sta_to_rt2x00_sta(sta);
305 txdesc->u.ht.wcid = sta_priv->wcid;
306 density = sta->ht_cap.ampdu_density;
307 }
308
309
310
311
312
313 if (txrate->flags & IEEE80211_TX_RC_MCS) {
314 txdesc->u.ht.mcs = txrate->idx;
315
316
317
318
319
320 if (sta && txdesc->u.ht.mcs > 7 &&
321 sta->smps_mode == IEEE80211_SMPS_DYNAMIC)
322 __set_bit(ENTRY_TXD_HT_MIMO_PS, &txdesc->flags);
323 } else {
324 txdesc->u.ht.mcs = rt2x00_get_rate_mcs(hwrate->mcs);
325 if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
326 txdesc->u.ht.mcs |= 0x08;
327 }
328
329 if (test_bit(CONFIG_HT_DISABLED, &rt2x00dev->flags)) {
330 if (!(tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT))
331 txdesc->u.ht.txop = TXOP_SIFS;
332 else
333 txdesc->u.ht.txop = TXOP_BACKOFF;
334
335
336 return;
337 }
338
339
340
341
342 if (tx_info->flags & IEEE80211_TX_CTL_STBC)
343 txdesc->u.ht.stbc = 1;
344
345
346
347
348
349 if (tx_info->flags & IEEE80211_TX_CTL_AMPDU &&
350 !(tx_info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)) {
351 __set_bit(ENTRY_TXD_HT_AMPDU, &txdesc->flags);
352 txdesc->u.ht.mpdu_density = density;
353 txdesc->u.ht.ba_size = 7;
354 }
355
356
357
358
359
360 if (txrate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH ||
361 txrate->flags & IEEE80211_TX_RC_DUP_DATA)
362 __set_bit(ENTRY_TXD_HT_BW_40, &txdesc->flags);
363 if (txrate->flags & IEEE80211_TX_RC_SHORT_GI)
364 __set_bit(ENTRY_TXD_HT_SHORT_GI, &txdesc->flags);
365
366
367
368
369
370
371
372
373
374
375 if (ieee80211_is_mgmt(hdr->frame_control) &&
376 !ieee80211_is_beacon(hdr->frame_control))
377 txdesc->u.ht.txop = TXOP_BACKOFF;
378 else if (!(tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT))
379 txdesc->u.ht.txop = TXOP_SIFS;
380 else
381 txdesc->u.ht.txop = TXOP_HTTXOP;
382}
383
384static void rt2x00queue_create_tx_descriptor(struct rt2x00_dev *rt2x00dev,
385 struct sk_buff *skb,
386 struct txentry_desc *txdesc,
387 struct ieee80211_sta *sta)
388{
389 struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
390 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
391 struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
392 struct ieee80211_rate *rate;
393 const struct rt2x00_rate *hwrate = NULL;
394
395 memset(txdesc, 0, sizeof(*txdesc));
396
397
398
399
400 txdesc->length = skb->len;
401 txdesc->header_length = ieee80211_get_hdrlen_from_skb(skb);
402
403
404
405
406 if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
407 __set_bit(ENTRY_TXD_ACK, &txdesc->flags);
408
409
410
411
412 if (ieee80211_is_rts(hdr->frame_control) ||
413 ieee80211_is_cts(hdr->frame_control)) {
414 __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
415 if (ieee80211_is_rts(hdr->frame_control))
416 __set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
417 else
418 __set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
419 if (tx_info->control.rts_cts_rate_idx >= 0)
420 rate =
421 ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
422 }
423
424
425
426
427 txdesc->retry_limit = tx_info->control.rates[0].count - 1;
428 if (txdesc->retry_limit >= rt2x00dev->long_retry)
429 __set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);
430
431
432
433
434 if (ieee80211_has_morefrags(hdr->frame_control)) {
435 __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
436 __set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
437 }
438
439
440
441
442 if (tx_info->flags & IEEE80211_TX_CTL_MORE_FRAMES)
443 __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
444
445
446
447
448
449 if ((ieee80211_is_beacon(hdr->frame_control) ||
450 ieee80211_is_probe_resp(hdr->frame_control)) &&
451 !(tx_info->flags & IEEE80211_TX_CTL_INJECTED))
452 __set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags);
453
454 if ((tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) &&
455 !test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags))
456 __set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
457
458
459
460
461 if (txrate->flags & IEEE80211_TX_RC_GREEN_FIELD)
462 txdesc->rate_mode = RATE_MODE_HT_GREENFIELD;
463 else if (txrate->flags & IEEE80211_TX_RC_MCS)
464 txdesc->rate_mode = RATE_MODE_HT_MIX;
465 else {
466 rate = ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
467 hwrate = rt2x00_get_rate(rate->hw_value);
468 if (hwrate->flags & DEV_RATE_OFDM)
469 txdesc->rate_mode = RATE_MODE_OFDM;
470 else
471 txdesc->rate_mode = RATE_MODE_CCK;
472 }
473
474
475
476
477 rt2x00crypto_create_tx_descriptor(rt2x00dev, skb, txdesc);
478 rt2x00queue_create_tx_descriptor_seq(rt2x00dev, skb, txdesc);
479
480 if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_HT_TX_DESC))
481 rt2x00queue_create_tx_descriptor_ht(rt2x00dev, skb, txdesc,
482 sta, hwrate);
483 else
484 rt2x00queue_create_tx_descriptor_plcp(rt2x00dev, skb, txdesc,
485 hwrate);
486}
487
488static int rt2x00queue_write_tx_data(struct queue_entry *entry,
489 struct txentry_desc *txdesc)
490{
491 struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
492
493
494
495
496
497
498 if (unlikely(rt2x00dev->ops->lib->get_entry_state &&
499 rt2x00dev->ops->lib->get_entry_state(entry))) {
500 rt2x00_err(rt2x00dev,
501 "Corrupt queue %d, accessing entry which is not ours\n"
502 "Please file bug report to %s\n",
503 entry->queue->qid, DRV_PROJECT);
504 return -EINVAL;
505 }
506
507
508
509
510 skb_push(entry->skb, rt2x00dev->extra_tx_headroom);
511 memset(entry->skb->data, 0, rt2x00dev->extra_tx_headroom);
512
513
514
515
516 if (rt2x00dev->ops->lib->write_tx_data)
517 rt2x00dev->ops->lib->write_tx_data(entry, txdesc);
518
519
520
521
522 if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DMA) &&
523 rt2x00queue_map_txskb(entry))
524 return -ENOMEM;
525
526 return 0;
527}
528
529static void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
530 struct txentry_desc *txdesc)
531{
532 struct data_queue *queue = entry->queue;
533
534 queue->rt2x00dev->ops->lib->write_tx_desc(entry, txdesc);
535
536
537
538
539
540 rt2x00debug_dump_frame(queue->rt2x00dev, DUMP_FRAME_TX, entry);
541}
542
543static void rt2x00queue_kick_tx_queue(struct data_queue *queue,
544 struct txentry_desc *txdesc)
545{
546
547
548
549
550
551
552
553
554
555 if (rt2x00queue_threshold(queue) ||
556 !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
557 queue->rt2x00dev->ops->lib->kick_queue(queue);
558}
559
560static void rt2x00queue_bar_check(struct queue_entry *entry)
561{
562 struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
563 struct ieee80211_bar *bar = (void *) (entry->skb->data +
564 rt2x00dev->extra_tx_headroom);
565 struct rt2x00_bar_list_entry *bar_entry;
566
567 if (likely(!ieee80211_is_back_req(bar->frame_control)))
568 return;
569
570 bar_entry = kmalloc(sizeof(*bar_entry), GFP_ATOMIC);
571
572
573
574
575
576
577 if (!bar_entry)
578 return;
579
580 bar_entry->entry = entry;
581 bar_entry->block_acked = 0;
582
583
584
585
586
587
588
589 memcpy(bar_entry->ra, bar->ra, sizeof(bar->ra));
590 memcpy(bar_entry->ta, bar->ta, sizeof(bar->ta));
591 bar_entry->control = bar->control;
592 bar_entry->start_seq_num = bar->start_seq_num;
593
594
595
596
597 spin_lock_bh(&rt2x00dev->bar_list_lock);
598 list_add_tail_rcu(&bar_entry->list, &rt2x00dev->bar_list);
599 spin_unlock_bh(&rt2x00dev->bar_list_lock);
600}
601
602int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
603 struct ieee80211_sta *sta, bool local)
604{
605 struct ieee80211_tx_info *tx_info;
606 struct queue_entry *entry;
607 struct txentry_desc txdesc;
608 struct skb_frame_desc *skbdesc;
609 u8 rate_idx, rate_flags;
610 int ret = 0;
611
612
613
614
615
616
617 rt2x00queue_create_tx_descriptor(queue->rt2x00dev, skb, &txdesc, sta);
618
619
620
621
622
623
624 tx_info = IEEE80211_SKB_CB(skb);
625 rate_idx = tx_info->control.rates[0].idx;
626 rate_flags = tx_info->control.rates[0].flags;
627 skbdesc = get_skb_frame_desc(skb);
628 memset(skbdesc, 0, sizeof(*skbdesc));
629 skbdesc->tx_rate_idx = rate_idx;
630 skbdesc->tx_rate_flags = rate_flags;
631
632 if (local)
633 skbdesc->flags |= SKBDESC_NOT_MAC80211;
634
635
636
637
638
639
640 if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
641 !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags)) {
642 if (rt2x00_has_cap_flag(queue->rt2x00dev, REQUIRE_COPY_IV))
643 rt2x00crypto_tx_copy_iv(skb, &txdesc);
644 else
645 rt2x00crypto_tx_remove_iv(skb, &txdesc);
646 }
647
648
649
650
651
652
653
654
655
656 if (rt2x00_has_cap_flag(queue->rt2x00dev, REQUIRE_L2PAD))
657 rt2x00queue_insert_l2pad(skb, txdesc.header_length);
658 else if (rt2x00_has_cap_flag(queue->rt2x00dev, REQUIRE_DMA))
659 rt2x00queue_align_frame(skb);
660
661
662
663
664 spin_lock(&queue->tx_lock);
665
666 if (unlikely(rt2x00queue_full(queue))) {
667 rt2x00_dbg(queue->rt2x00dev, "Dropping frame due to full tx queue %d\n",
668 queue->qid);
669 ret = -ENOBUFS;
670 goto out;
671 }
672
673 entry = rt2x00queue_get_entry(queue, Q_INDEX);
674
675 if (unlikely(test_and_set_bit(ENTRY_OWNER_DEVICE_DATA,
676 &entry->flags))) {
677 rt2x00_err(queue->rt2x00dev,
678 "Arrived at non-free entry in the non-full queue %d\n"
679 "Please file bug report to %s\n",
680 queue->qid, DRV_PROJECT);
681 ret = -EINVAL;
682 goto out;
683 }
684
685 entry->skb = skb;
686
687
688
689
690
691
692 if (unlikely(rt2x00queue_write_tx_data(entry, &txdesc))) {
693 clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
694 entry->skb = NULL;
695 ret = -EIO;
696 goto out;
697 }
698
699
700
701
702 rt2x00queue_bar_check(entry);
703
704 set_bit(ENTRY_DATA_PENDING, &entry->flags);
705
706 rt2x00queue_index_inc(entry, Q_INDEX);
707 rt2x00queue_write_tx_descriptor(entry, &txdesc);
708 rt2x00queue_kick_tx_queue(queue, &txdesc);
709
710out:
711
712
713
714
715
716 if (rt2x00queue_threshold(queue))
717 rt2x00queue_pause_queue(queue);
718
719 spin_unlock(&queue->tx_lock);
720 return ret;
721}
722
723int rt2x00queue_clear_beacon(struct rt2x00_dev *rt2x00dev,
724 struct ieee80211_vif *vif)
725{
726 struct rt2x00_intf *intf = vif_to_intf(vif);
727
728 if (unlikely(!intf->beacon))
729 return -ENOBUFS;
730
731
732
733
734 rt2x00queue_free_skb(intf->beacon);
735
736
737
738
739
740 if (rt2x00dev->ops->lib->clear_beacon)
741 rt2x00dev->ops->lib->clear_beacon(intf->beacon);
742
743 return 0;
744}
745
746int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
747 struct ieee80211_vif *vif)
748{
749 struct rt2x00_intf *intf = vif_to_intf(vif);
750 struct skb_frame_desc *skbdesc;
751 struct txentry_desc txdesc;
752
753 if (unlikely(!intf->beacon))
754 return -ENOBUFS;
755
756
757
758
759 rt2x00queue_free_skb(intf->beacon);
760
761 intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
762 if (!intf->beacon->skb)
763 return -ENOMEM;
764
765
766
767
768
769
770 rt2x00queue_create_tx_descriptor(rt2x00dev, intf->beacon->skb, &txdesc, NULL);
771
772
773
774
775 skbdesc = get_skb_frame_desc(intf->beacon->skb);
776 memset(skbdesc, 0, sizeof(*skbdesc));
777
778
779
780
781 rt2x00dev->ops->lib->write_beacon(intf->beacon, &txdesc);
782
783 return 0;
784
785}
786
787bool rt2x00queue_for_each_entry(struct data_queue *queue,
788 enum queue_index start,
789 enum queue_index end,
790 void *data,
791 bool (*fn)(struct queue_entry *entry,
792 void *data))
793{
794 unsigned long irqflags;
795 unsigned int index_start;
796 unsigned int index_end;
797 unsigned int i;
798
799 if (unlikely(start >= Q_INDEX_MAX || end >= Q_INDEX_MAX)) {
800 rt2x00_err(queue->rt2x00dev,
801 "Entry requested from invalid index range (%d - %d)\n",
802 start, end);
803 return true;
804 }
805
806
807
808
809
810
811
812 spin_lock_irqsave(&queue->index_lock, irqflags);
813 index_start = queue->index[start];
814 index_end = queue->index[end];
815 spin_unlock_irqrestore(&queue->index_lock, irqflags);
816
817
818
819
820
821 if (index_start < index_end) {
822 for (i = index_start; i < index_end; i++) {
823 if (fn(&queue->entries[i], data))
824 return true;
825 }
826 } else {
827 for (i = index_start; i < queue->limit; i++) {
828 if (fn(&queue->entries[i], data))
829 return true;
830 }
831
832 for (i = 0; i < index_end; i++) {
833 if (fn(&queue->entries[i], data))
834 return true;
835 }
836 }
837
838 return false;
839}
840EXPORT_SYMBOL_GPL(rt2x00queue_for_each_entry);
841
842struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
843 enum queue_index index)
844{
845 struct queue_entry *entry;
846 unsigned long irqflags;
847
848 if (unlikely(index >= Q_INDEX_MAX)) {
849 rt2x00_err(queue->rt2x00dev, "Entry requested from invalid index type (%d)\n",
850 index);
851 return NULL;
852 }
853
854 spin_lock_irqsave(&queue->index_lock, irqflags);
855
856 entry = &queue->entries[queue->index[index]];
857
858 spin_unlock_irqrestore(&queue->index_lock, irqflags);
859
860 return entry;
861}
862EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);
863
864void rt2x00queue_index_inc(struct queue_entry *entry, enum queue_index index)
865{
866 struct data_queue *queue = entry->queue;
867 unsigned long irqflags;
868
869 if (unlikely(index >= Q_INDEX_MAX)) {
870 rt2x00_err(queue->rt2x00dev,
871 "Index change on invalid index type (%d)\n", index);
872 return;
873 }
874
875 spin_lock_irqsave(&queue->index_lock, irqflags);
876
877 queue->index[index]++;
878 if (queue->index[index] >= queue->limit)
879 queue->index[index] = 0;
880
881 entry->last_action = jiffies;
882
883 if (index == Q_INDEX) {
884 queue->length++;
885 } else if (index == Q_INDEX_DONE) {
886 queue->length--;
887 queue->count++;
888 }
889
890 spin_unlock_irqrestore(&queue->index_lock, irqflags);
891}
892
893static void rt2x00queue_pause_queue_nocheck(struct data_queue *queue)
894{
895 switch (queue->qid) {
896 case QID_AC_VO:
897 case QID_AC_VI:
898 case QID_AC_BE:
899 case QID_AC_BK:
900
901
902
903
904 ieee80211_stop_queue(queue->rt2x00dev->hw, queue->qid);
905 break;
906 default:
907 break;
908 }
909}
910void rt2x00queue_pause_queue(struct data_queue *queue)
911{
912 if (!test_bit(DEVICE_STATE_PRESENT, &queue->rt2x00dev->flags) ||
913 !test_bit(QUEUE_STARTED, &queue->flags) ||
914 test_and_set_bit(QUEUE_PAUSED, &queue->flags))
915 return;
916
917 rt2x00queue_pause_queue_nocheck(queue);
918}
919EXPORT_SYMBOL_GPL(rt2x00queue_pause_queue);
920
921void rt2x00queue_unpause_queue(struct data_queue *queue)
922{
923 if (!test_bit(DEVICE_STATE_PRESENT, &queue->rt2x00dev->flags) ||
924 !test_bit(QUEUE_STARTED, &queue->flags) ||
925 !test_and_clear_bit(QUEUE_PAUSED, &queue->flags))
926 return;
927
928 switch (queue->qid) {
929 case QID_AC_VO:
930 case QID_AC_VI:
931 case QID_AC_BE:
932 case QID_AC_BK:
933
934
935
936
937 ieee80211_wake_queue(queue->rt2x00dev->hw, queue->qid);
938 break;
939 case QID_RX:
940
941
942
943
944 queue->rt2x00dev->ops->lib->kick_queue(queue);
945 break;
946 default:
947 break;
948 }
949}
950EXPORT_SYMBOL_GPL(rt2x00queue_unpause_queue);
951
952void rt2x00queue_start_queue(struct data_queue *queue)
953{
954 mutex_lock(&queue->status_lock);
955
956 if (!test_bit(DEVICE_STATE_PRESENT, &queue->rt2x00dev->flags) ||
957 test_and_set_bit(QUEUE_STARTED, &queue->flags)) {
958 mutex_unlock(&queue->status_lock);
959 return;
960 }
961
962 set_bit(QUEUE_PAUSED, &queue->flags);
963
964 queue->rt2x00dev->ops->lib->start_queue(queue);
965
966 rt2x00queue_unpause_queue(queue);
967
968 mutex_unlock(&queue->status_lock);
969}
970EXPORT_SYMBOL_GPL(rt2x00queue_start_queue);
971
972void rt2x00queue_stop_queue(struct data_queue *queue)
973{
974 mutex_lock(&queue->status_lock);
975
976 if (!test_and_clear_bit(QUEUE_STARTED, &queue->flags)) {
977 mutex_unlock(&queue->status_lock);
978 return;
979 }
980
981 rt2x00queue_pause_queue_nocheck(queue);
982
983 queue->rt2x00dev->ops->lib->stop_queue(queue);
984
985 mutex_unlock(&queue->status_lock);
986}
987EXPORT_SYMBOL_GPL(rt2x00queue_stop_queue);
988
989void rt2x00queue_flush_queue(struct data_queue *queue, bool drop)
990{
991 bool tx_queue =
992 (queue->qid == QID_AC_VO) ||
993 (queue->qid == QID_AC_VI) ||
994 (queue->qid == QID_AC_BE) ||
995 (queue->qid == QID_AC_BK);
996
997 if (rt2x00queue_empty(queue))
998 return;
999
1000
1001
1002
1003
1004
1005
1006 if (!drop && tx_queue)
1007 queue->rt2x00dev->ops->lib->kick_queue(queue);
1008
1009
1010
1011
1012
1013
1014 if (likely(queue->rt2x00dev->ops->lib->flush_queue))
1015 queue->rt2x00dev->ops->lib->flush_queue(queue, drop);
1016
1017
1018
1019
1020 if (unlikely(!rt2x00queue_empty(queue)))
1021 rt2x00_warn(queue->rt2x00dev, "Queue %d failed to flush\n",
1022 queue->qid);
1023}
1024EXPORT_SYMBOL_GPL(rt2x00queue_flush_queue);
1025
1026void rt2x00queue_start_queues(struct rt2x00_dev *rt2x00dev)
1027{
1028 struct data_queue *queue;
1029
1030
1031
1032
1033
1034 tx_queue_for_each(rt2x00dev, queue)
1035 rt2x00queue_start_queue(queue);
1036
1037 rt2x00queue_start_queue(rt2x00dev->rx);
1038}
1039EXPORT_SYMBOL_GPL(rt2x00queue_start_queues);
1040
1041void rt2x00queue_stop_queues(struct rt2x00_dev *rt2x00dev)
1042{
1043 struct data_queue *queue;
1044
1045
1046
1047
1048
1049
1050
1051 ieee80211_stop_queues(rt2x00dev->hw);
1052
1053 tx_queue_for_each(rt2x00dev, queue)
1054 rt2x00queue_stop_queue(queue);
1055
1056 rt2x00queue_stop_queue(rt2x00dev->rx);
1057}
1058EXPORT_SYMBOL_GPL(rt2x00queue_stop_queues);
1059
1060void rt2x00queue_flush_queues(struct rt2x00_dev *rt2x00dev, bool drop)
1061{
1062 struct data_queue *queue;
1063
1064 tx_queue_for_each(rt2x00dev, queue)
1065 rt2x00queue_flush_queue(queue, drop);
1066
1067 rt2x00queue_flush_queue(rt2x00dev->rx, drop);
1068}
1069EXPORT_SYMBOL_GPL(rt2x00queue_flush_queues);
1070
1071static void rt2x00queue_reset(struct data_queue *queue)
1072{
1073 unsigned long irqflags;
1074 unsigned int i;
1075
1076 spin_lock_irqsave(&queue->index_lock, irqflags);
1077
1078 queue->count = 0;
1079 queue->length = 0;
1080
1081 for (i = 0; i < Q_INDEX_MAX; i++)
1082 queue->index[i] = 0;
1083
1084 spin_unlock_irqrestore(&queue->index_lock, irqflags);
1085}
1086
1087void rt2x00queue_init_queues(struct rt2x00_dev *rt2x00dev)
1088{
1089 struct data_queue *queue;
1090 unsigned int i;
1091
1092 queue_for_each(rt2x00dev, queue) {
1093 rt2x00queue_reset(queue);
1094
1095 for (i = 0; i < queue->limit; i++)
1096 rt2x00dev->ops->lib->clear_entry(&queue->entries[i]);
1097 }
1098}
1099
1100static int rt2x00queue_alloc_entries(struct data_queue *queue)
1101{
1102 struct queue_entry *entries;
1103 unsigned int entry_size;
1104 unsigned int i;
1105
1106 rt2x00queue_reset(queue);
1107
1108
1109
1110
1111 entry_size = sizeof(*entries) + queue->priv_size;
1112 entries = kcalloc(queue->limit, entry_size, GFP_KERNEL);
1113 if (!entries)
1114 return -ENOMEM;
1115
1116#define QUEUE_ENTRY_PRIV_OFFSET(__base, __index, __limit, __esize, __psize) \
1117 (((char *)(__base)) + ((__limit) * (__esize)) + \
1118 ((__index) * (__psize)))
1119
1120 for (i = 0; i < queue->limit; i++) {
1121 entries[i].flags = 0;
1122 entries[i].queue = queue;
1123 entries[i].skb = NULL;
1124 entries[i].entry_idx = i;
1125 entries[i].priv_data =
1126 QUEUE_ENTRY_PRIV_OFFSET(entries, i, queue->limit,
1127 sizeof(*entries), queue->priv_size);
1128 }
1129
1130#undef QUEUE_ENTRY_PRIV_OFFSET
1131
1132 queue->entries = entries;
1133
1134 return 0;
1135}
1136
1137static void rt2x00queue_free_skbs(struct data_queue *queue)
1138{
1139 unsigned int i;
1140
1141 if (!queue->entries)
1142 return;
1143
1144 for (i = 0; i < queue->limit; i++) {
1145 rt2x00queue_free_skb(&queue->entries[i]);
1146 }
1147}
1148
1149static int rt2x00queue_alloc_rxskbs(struct data_queue *queue)
1150{
1151 unsigned int i;
1152 struct sk_buff *skb;
1153
1154 for (i = 0; i < queue->limit; i++) {
1155 skb = rt2x00queue_alloc_rxskb(&queue->entries[i], GFP_KERNEL);
1156 if (!skb)
1157 return -ENOMEM;
1158 queue->entries[i].skb = skb;
1159 }
1160
1161 return 0;
1162}
1163
1164int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
1165{
1166 struct data_queue *queue;
1167 int status;
1168
1169 status = rt2x00queue_alloc_entries(rt2x00dev->rx);
1170 if (status)
1171 goto exit;
1172
1173 tx_queue_for_each(rt2x00dev, queue) {
1174 status = rt2x00queue_alloc_entries(queue);
1175 if (status)
1176 goto exit;
1177 }
1178
1179 status = rt2x00queue_alloc_entries(rt2x00dev->bcn);
1180 if (status)
1181 goto exit;
1182
1183 if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_ATIM_QUEUE)) {
1184 status = rt2x00queue_alloc_entries(rt2x00dev->atim);
1185 if (status)
1186 goto exit;
1187 }
1188
1189 status = rt2x00queue_alloc_rxskbs(rt2x00dev->rx);
1190 if (status)
1191 goto exit;
1192
1193 return 0;
1194
1195exit:
1196 rt2x00_err(rt2x00dev, "Queue entries allocation failed\n");
1197
1198 rt2x00queue_uninitialize(rt2x00dev);
1199
1200 return status;
1201}
1202
1203void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
1204{
1205 struct data_queue *queue;
1206
1207 rt2x00queue_free_skbs(rt2x00dev->rx);
1208
1209 queue_for_each(rt2x00dev, queue) {
1210 kfree(queue->entries);
1211 queue->entries = NULL;
1212 }
1213}
1214
1215static void rt2x00queue_init(struct rt2x00_dev *rt2x00dev,
1216 struct data_queue *queue, enum data_queue_qid qid)
1217{
1218 mutex_init(&queue->status_lock);
1219 spin_lock_init(&queue->tx_lock);
1220 spin_lock_init(&queue->index_lock);
1221
1222 queue->rt2x00dev = rt2x00dev;
1223 queue->qid = qid;
1224 queue->txop = 0;
1225 queue->aifs = 2;
1226 queue->cw_min = 5;
1227 queue->cw_max = 10;
1228
1229 rt2x00dev->ops->queue_init(queue);
1230
1231 queue->threshold = DIV_ROUND_UP(queue->limit, 10);
1232}
1233
1234int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
1235{
1236 struct data_queue *queue;
1237 enum data_queue_qid qid;
1238 unsigned int req_atim =
1239 rt2x00_has_cap_flag(rt2x00dev, REQUIRE_ATIM_QUEUE);
1240
1241
1242
1243
1244
1245
1246
1247
1248 rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
1249
1250 queue = kcalloc(rt2x00dev->data_queues, sizeof(*queue), GFP_KERNEL);
1251 if (!queue)
1252 return -ENOMEM;
1253
1254
1255
1256
1257 rt2x00dev->rx = queue;
1258 rt2x00dev->tx = &queue[1];
1259 rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
1260 rt2x00dev->atim = req_atim ? &queue[2 + rt2x00dev->ops->tx_queues] : NULL;
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271 rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
1272
1273 qid = QID_AC_VO;
1274 tx_queue_for_each(rt2x00dev, queue)
1275 rt2x00queue_init(rt2x00dev, queue, qid++);
1276
1277 rt2x00queue_init(rt2x00dev, rt2x00dev->bcn, QID_BEACON);
1278 if (req_atim)
1279 rt2x00queue_init(rt2x00dev, rt2x00dev->atim, QID_ATIM);
1280
1281 return 0;
1282}
1283
1284void rt2x00queue_free(struct rt2x00_dev *rt2x00dev)
1285{
1286 kfree(rt2x00dev->rx);
1287 rt2x00dev->rx = NULL;
1288 rt2x00dev->tx = NULL;
1289 rt2x00dev->bcn = NULL;
1290}
1291