Commit | Line | Data |
---|---|---|
4bd43f50 LR |
1 | /* |
2 | * Copyright (c) 2007-2008 Atheros Communications Inc. | |
3 | * | |
4 | * Permission to use, copy, modify, and/or distribute this software for any | |
5 | * purpose with or without fee is hereby granted, provided that the above | |
6 | * copyright notice and this permission notice appear in all copies. | |
7 | * | |
8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES | |
9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF | |
10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR | |
11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES | |
12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN | |
13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF | |
14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. | |
15 | */ | |
16 | /* */ | |
17 | /* Module Name : queue.c */ | |
18 | /* */ | |
19 | /* Abstract */ | |
20 | /* This module contains queue management functions. */ | |
21 | /* */ | |
22 | /* NOTES */ | |
23 | /* None */ | |
24 | /* */ | |
25 | /************************************************************************/ | |
26 | #include "cprecomp.h" | |
27 | #include "queue.h" | |
28 | ||
29 | ||
30 | struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size) | |
31 | { | |
32 | struct zsQueue* q; | |
33 | ||
92363b52 JP |
34 | q = (struct zsQueue*)zfwMemAllocate(dev, sizeof(struct zsQueue) |
35 | + (sizeof(struct zsQueueCell)*(size-1))); | |
36 | if (q != NULL) | |
4bd43f50 LR |
37 | { |
38 | q->size = size; | |
39 | q->sizeMask = size-1; | |
40 | q->head = 0; | |
41 | q->tail = 0; | |
42 | } | |
43 | return q; | |
44 | } | |
45 | ||
46 | void zfQueueDestroy(zdev_t* dev, struct zsQueue* q) | |
47 | { | |
48 | u16_t size = sizeof(struct zsQueue) + (sizeof(struct zsQueueCell)*(q->size-1)); | |
49 | ||
50 | zfQueueFlush(dev, q); | |
51 | zfwMemFree(dev, q, size); | |
52 | ||
53 | return; | |
54 | } | |
55 | ||
56 | u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick) | |
57 | { | |
58 | u16_t ret = ZM_ERR_QUEUE_FULL; | |
59 | ||
60 | zm_msg0_mm(ZM_LV_1, "zfQueuePutNcs()"); | |
61 | ||
62 | if (((q->tail+1)&q->sizeMask) != q->head) | |
63 | { | |
64 | q->cell[q->tail].buf = buf; | |
65 | q->cell[q->tail].tick = tick; | |
66 | q->tail = (q->tail+1) & q->sizeMask; | |
67 | ret = ZM_SUCCESS; | |
68 | } | |
69 | ||
70 | return ret; | |
71 | } | |
72 | ||
73 | u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick) | |
74 | { | |
75 | u16_t ret; | |
76 | zmw_declare_for_critical_section(); | |
77 | ||
78 | zmw_enter_critical_section(dev); | |
79 | ||
80 | ret = zfQueuePutNcs(dev, q, buf, tick); | |
81 | ||
82 | zmw_leave_critical_section(dev); | |
83 | ||
84 | return ret; | |
85 | } | |
86 | ||
87 | zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q) | |
88 | { | |
89 | zbuf_t* buf = NULL; | |
90 | zmw_declare_for_critical_section(); | |
91 | ||
92 | zmw_enter_critical_section(dev); | |
93 | ||
94 | if (q->head != q->tail) | |
95 | { | |
96 | buf = q->cell[q->head].buf; | |
97 | q->head = (q->head+1) & q->sizeMask; | |
98 | } | |
99 | ||
100 | zmw_leave_critical_section(dev); | |
101 | ||
102 | return buf; | |
103 | } | |
104 | ||
105 | u16_t zfCompareDstwithBuf(zdev_t* dev, zbuf_t* buf, u8_t* addr) | |
106 | { | |
107 | u16_t i; | |
108 | u8_t dst[6]; | |
109 | ||
110 | for (i=0; i<6; i++) | |
111 | { | |
112 | dst[i] = zmw_buf_readb(dev, buf, i); | |
113 | if (dst[i] != addr[i]) | |
114 | { | |
115 | return 1+i; | |
116 | } | |
117 | } | |
118 | ||
119 | return 0; | |
120 | } | |
121 | ||
122 | ||
123 | zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb) | |
124 | { | |
125 | zbuf_t* buf; | |
126 | zbuf_t* retBuf = NULL; | |
127 | u16_t index, next; | |
128 | zmw_declare_for_critical_section(); | |
129 | ||
130 | *mb = 0; | |
131 | ||
132 | zmw_enter_critical_section(dev); | |
133 | ||
134 | index = q->head; | |
135 | ||
136 | while (1) | |
137 | { | |
138 | if (index != q->tail) | |
139 | { | |
140 | buf = q->cell[index].buf; | |
141 | ||
142 | //if buf's detination address == input addr | |
143 | if (zfCompareDstwithBuf(dev, buf, addr) == 0) | |
144 | { | |
145 | retBuf = buf; | |
146 | //Get it, and trace the whole queue to calculate more bit | |
147 | while ((next =((index+1)&q->sizeMask)) != q->tail) | |
148 | { | |
149 | q->cell[index].buf = q->cell[next].buf; | |
150 | q->cell[index].tick = q->cell[next].tick; | |
151 | ||
152 | if ((*mb == 0) && (zfCompareDstwithBuf(dev, | |
153 | q->cell[next].buf, addr) == 0)) | |
154 | { | |
155 | *mb = 1; | |
156 | } | |
157 | ||
158 | index = next; | |
159 | } | |
160 | q->tail = (q->tail-1) & q->sizeMask; | |
161 | ||
162 | zmw_leave_critical_section(dev); | |
163 | return retBuf; | |
164 | } | |
165 | index = (index + 1) & q->sizeMask; | |
166 | } //if (index != q->tail) | |
167 | else | |
168 | { | |
169 | break; | |
170 | } | |
171 | } | |
172 | ||
173 | zmw_leave_critical_section(dev); | |
174 | ||
175 | return retBuf; | |
176 | ||
177 | } | |
178 | ||
179 | void zfQueueFlush(zdev_t* dev, struct zsQueue* q) | |
180 | { | |
181 | zbuf_t* buf; | |
182 | ||
183 | while ((buf = zfQueueGet(dev, q)) != NULL) | |
184 | { | |
185 | zfwBufFree(dev, buf, 0); | |
186 | } | |
187 | ||
188 | return; | |
189 | } | |
190 | ||
191 | void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge) | |
192 | { | |
193 | zbuf_t* buf; | |
194 | u32_t buftick; | |
195 | zmw_declare_for_critical_section(); | |
196 | ||
197 | while (1) | |
198 | { | |
199 | buf = NULL; | |
200 | zmw_enter_critical_section(dev); | |
201 | ||
202 | if (q->head != q->tail) | |
203 | { | |
204 | buftick = q->cell[q->head].tick; | |
205 | if (((tick - buftick)*ZM_MS_PER_TICK) > msAge) | |
206 | { | |
207 | buf = q->cell[q->head].buf; | |
208 | q->head = (q->head+1) & q->sizeMask; | |
209 | } | |
210 | } | |
211 | ||
212 | zmw_leave_critical_section(dev); | |
213 | ||
214 | if (buf != NULL) | |
215 | { | |
216 | zm_msg0_mm(ZM_LV_0, "Age frame in queue!"); | |
217 | zfwBufFree(dev, buf, 0); | |
218 | } | |
219 | else | |
220 | { | |
221 | break; | |
222 | } | |
223 | } | |
224 | return; | |
225 | } | |
226 | ||
227 | ||
228 | u8_t zfQueueRemovewithIndex(zdev_t* dev, struct zsQueue* q, u16_t index, u8_t* addr) | |
229 | { | |
230 | u16_t next; | |
231 | u8_t mb = 0; | |
232 | ||
233 | //trace the whole queue to calculate more bit | |
234 | while ((next =((index+1)&q->sizeMask)) != q->tail) | |
235 | { | |
236 | q->cell[index].buf = q->cell[next].buf; | |
237 | q->cell[index].tick = q->cell[next].tick; | |
238 | ||
239 | if ((mb == 0) && (zfCompareDstwithBuf(dev, | |
240 | q->cell[next].buf, addr) == 0)) | |
241 | { | |
242 | mb = 1; | |
243 | } | |
244 | ||
245 | index = next; | |
246 | } | |
247 | q->tail = (q->tail-1) & q->sizeMask; | |
248 | ||
249 | return mb; | |
250 | ||
251 | } | |
252 | ||
253 | void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q, | |
254 | u8_t* uniBitMap, u16_t* highestByte) | |
255 | { | |
256 | zbuf_t* psBuf; | |
257 | u8_t dst[6]; | |
258 | u16_t id, aid, index, i; | |
259 | u16_t bitPosition; | |
260 | u16_t bytePosition; | |
261 | zmw_get_wlan_dev(dev); | |
262 | zmw_declare_for_critical_section(); | |
263 | ||
264 | zmw_enter_critical_section(dev); | |
265 | ||
266 | index = q->head; | |
267 | ||
268 | while (index != q->tail) | |
269 | { | |
270 | psBuf = q->cell[index].buf; | |
271 | for (i=0; i<6; i++) | |
272 | { | |
273 | dst[i] = zmw_buf_readb(dev, psBuf, i); | |
274 | } | |
275 | /* TODO : use u8_t* fot MAC address */ | |
276 | if (((id = zfApFindSta(dev, (u16_t*)dst)) != 0xffff) | |
277 | && (wd->ap.staTable[id].psMode != 0)) | |
278 | { | |
279 | /* Calculate PVB only when all AC are delivery-enabled */ | |
280 | if ((wd->ap.staTable[id].qosInfo & 0xf) == 0xf) | |
281 | { | |
282 | aid = id + 1; | |
283 | bitPosition = (1 << (aid & 0x7)); | |
284 | bytePosition = (aid >> 3); | |
285 | uniBitMap[bytePosition] |= bitPosition; | |
286 | ||
287 | if (bytePosition>*highestByte) | |
288 | { | |
289 | *highestByte = bytePosition; | |
290 | } | |
291 | } | |
292 | index = (index+1) & q->sizeMask; | |
293 | } | |
294 | else | |
295 | { | |
296 | /* Free garbage UAPSD frame */ | |
297 | zfQueueRemovewithIndex(dev, q, index, dst); | |
298 | zfwBufFree(dev, psBuf, 0); | |
299 | } | |
300 | } | |
301 | zmw_leave_critical_section(dev); | |
302 | ||
303 | return; | |
304 | } |