rt2x00: Fix queue index handling
[linux-2.6.git] / drivers / net / wireless / rt2x00 / rt2x00queue.c
1 /*
2         Copyright (C) 2004 - 2008 rt2x00 SourceForge Project
3         <http://rt2x00.serialmonkey.com>
4
5         This program is free software; you can redistribute it and/or modify
6         it under the terms of the GNU General Public License as published by
7         the Free Software Foundation; either version 2 of the License, or
8         (at your option) any later version.
9
10         This program is distributed in the hope that it will be useful,
11         but WITHOUT ANY WARRANTY; without even the implied warranty of
12         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13         GNU General Public License for more details.
14
15         You should have received a copy of the GNU General Public License
16         along with this program; if not, write to the
17         Free Software Foundation, Inc.,
18         59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
19  */
20
21 /*
22         Module: rt2x00lib
23         Abstract: rt2x00 queue specific routines.
24  */
25
26 #include <linux/kernel.h>
27 #include <linux/module.h>
28
29 #include "rt2x00.h"
30 #include "rt2x00lib.h"
31
32 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
33                                          const unsigned int queue)
34 {
35         int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
36
37         if (queue < rt2x00dev->hw->queues && rt2x00dev->tx)
38                 return &rt2x00dev->tx[queue];
39
40         if (!rt2x00dev->bcn)
41                 return NULL;
42
43         if (queue == RT2X00_BCN_QUEUE_BEACON)
44                 return &rt2x00dev->bcn[0];
45         else if (queue == RT2X00_BCN_QUEUE_ATIM && atim)
46                 return &rt2x00dev->bcn[1];
47
48         return NULL;
49 }
50 EXPORT_SYMBOL_GPL(rt2x00queue_get_queue);
51
52 struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
53                                           enum queue_index index)
54 {
55         struct queue_entry *entry;
56
57         if (unlikely(index >= Q_INDEX_MAX)) {
58                 ERROR(queue->rt2x00dev,
59                       "Entry requested from invalid index type (%d)\n", index);
60                 return NULL;
61         }
62
63         spin_lock(&queue->lock);
64
65         entry = &queue->entries[queue->index[index]];
66
67         spin_unlock(&queue->lock);
68
69         return entry;
70 }
71 EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);
72
73 void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index)
74 {
75         if (unlikely(index >= Q_INDEX_MAX)) {
76                 ERROR(queue->rt2x00dev,
77                       "Index change on invalid index type (%d)\n", index);
78                 return;
79         }
80
81         spin_lock(&queue->lock);
82
83         queue->index[index]++;
84         if (queue->index[index] >= queue->limit)
85                 queue->index[index] = 0;
86
87         if (index == Q_INDEX) {
88                 queue->length++;
89         } else if (index == Q_INDEX_DONE) {
90                 queue->length--;
91                 queue->count ++;
92         }
93
94         spin_unlock(&queue->lock);
95 }
96 EXPORT_SYMBOL_GPL(rt2x00queue_index_inc);
97
98 static void rt2x00queue_reset(struct data_queue *queue)
99 {
100         spin_lock(&queue->lock);
101
102         queue->count = 0;
103         queue->length = 0;
104         memset(queue->index, 0, sizeof(queue->index));
105
106         spin_unlock(&queue->lock);
107 }
108
109 void rt2x00queue_init_rx(struct rt2x00_dev *rt2x00dev)
110 {
111         struct data_queue *queue = rt2x00dev->rx;
112         unsigned int i;
113
114         rt2x00queue_reset(queue);
115
116         if (!rt2x00dev->ops->lib->init_rxentry)
117                 return;
118
119         for (i = 0; i < queue->limit; i++)
120                 rt2x00dev->ops->lib->init_rxentry(rt2x00dev,
121                                                   &queue->entries[i]);
122 }
123
124 void rt2x00queue_init_tx(struct rt2x00_dev *rt2x00dev)
125 {
126         struct data_queue *queue;
127         unsigned int i;
128
129         txall_queue_for_each(rt2x00dev, queue) {
130                 rt2x00queue_reset(queue);
131
132                 if (!rt2x00dev->ops->lib->init_txentry)
133                         continue;
134
135                 for (i = 0; i < queue->limit; i++)
136                         rt2x00dev->ops->lib->init_txentry(rt2x00dev,
137                                                           &queue->entries[i]);
138         }
139 }
140
141 static int rt2x00queue_alloc_entries(struct data_queue *queue,
142                                      const struct data_queue_desc *qdesc)
143 {
144         struct queue_entry *entries;
145         unsigned int entry_size;
146         unsigned int i;
147
148         rt2x00queue_reset(queue);
149
150         queue->limit = qdesc->entry_num;
151         queue->data_size = qdesc->data_size;
152         queue->desc_size = qdesc->desc_size;
153
154         /*
155          * Allocate all queue entries.
156          */
157         entry_size = sizeof(*entries) + qdesc->priv_size;
158         entries = kzalloc(queue->limit * entry_size, GFP_KERNEL);
159         if (!entries)
160                 return -ENOMEM;
161
162 #define QUEUE_ENTRY_PRIV_OFFSET(__base, __index, __limit, __esize, __psize) \
163         ( (__base) + ((__limit) * (__esize)) + ((__index) * (__psize)) )
164
165         for (i = 0; i < queue->limit; i++) {
166                 entries[i].flags = 0;
167                 entries[i].queue = queue;
168                 entries[i].skb = NULL;
169                 entries[i].entry_idx = i;
170                 entries[i].priv_data =
171                     QUEUE_ENTRY_PRIV_OFFSET(entries, i, queue->limit,
172                                             sizeof(*entries), qdesc->priv_size);
173         }
174
175 #undef QUEUE_ENTRY_PRIV_OFFSET
176
177         queue->entries = entries;
178
179         return 0;
180 }
181
182 int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
183 {
184         struct data_queue *queue;
185         int status;
186
187
188         status = rt2x00queue_alloc_entries(rt2x00dev->rx, rt2x00dev->ops->rx);
189         if (status)
190                 goto exit;
191
192         tx_queue_for_each(rt2x00dev, queue) {
193                 status = rt2x00queue_alloc_entries(queue, rt2x00dev->ops->tx);
194                 if (status)
195                         goto exit;
196         }
197
198         status = rt2x00queue_alloc_entries(rt2x00dev->bcn, rt2x00dev->ops->bcn);
199         if (status)
200                 goto exit;
201
202         if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
203                 return 0;
204
205         status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
206                                            rt2x00dev->ops->atim);
207         if (status)
208                 goto exit;
209
210         return 0;
211
212 exit:
213         ERROR(rt2x00dev, "Queue entries allocation failed.\n");
214
215         rt2x00queue_uninitialize(rt2x00dev);
216
217         return status;
218 }
219
220 void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
221 {
222         struct data_queue *queue;
223
224         queue_for_each(rt2x00dev, queue) {
225                 kfree(queue->entries);
226                 queue->entries = NULL;
227         }
228 }
229
230 int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
231 {
232         struct data_queue *queue;
233         enum data_queue_qid qid;
234         unsigned int req_atim =
235             !!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
236
237         /*
238          * We need the following queues:
239          * RX: 1
240          * TX: hw->queues
241          * Beacon: 1
242          * Atim: 1 (if required)
243          */
244         rt2x00dev->data_queues = 2 + rt2x00dev->hw->queues + req_atim;
245
246         queue = kzalloc(rt2x00dev->data_queues * sizeof(*queue), GFP_KERNEL);
247         if (!queue) {
248                 ERROR(rt2x00dev, "Queue allocation failed.\n");
249                 return -ENOMEM;
250         }
251
252         /*
253          * Initialize pointers
254          */
255         rt2x00dev->rx = queue;
256         rt2x00dev->tx = &queue[1];
257         rt2x00dev->bcn = &queue[1 + rt2x00dev->hw->queues];
258
259         /*
260          * Initialize queue parameters.
261          * RX: qid = QID_RX
262          * TX: qid = QID_AC_BE + index
263          * TX: cw_min: 2^5 = 32.
264          * TX: cw_max: 2^10 = 1024.
265          * BCN & Atim: qid = QID_MGMT
266          */
267         qid = QID_AC_BE;
268         queue_for_each(rt2x00dev, queue) {
269                 spin_lock_init(&queue->lock);
270
271                 queue->rt2x00dev = rt2x00dev;
272                 queue->qid = qid++;
273                 queue->aifs = 2;
274                 queue->cw_min = 5;
275                 queue->cw_max = 10;
276         }
277
278         /*
279          * Fix non-TX data qid's
280          */
281         rt2x00dev->rx->qid = QID_RX;
282         rt2x00dev->bcn[0].qid = QID_MGMT;
283         if (req_atim)
284                 rt2x00dev->bcn[1].qid = QID_MGMT;
285
286         return 0;
287 }
288
289 void rt2x00queue_free(struct rt2x00_dev *rt2x00dev)
290 {
291         kfree(rt2x00dev->rx);
292         rt2x00dev->rx = NULL;
293         rt2x00dev->tx = NULL;
294         rt2x00dev->bcn = NULL;
295 }