1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26#include "cprecomp.h"
27#include "queue.h"
28
29
30struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size)
31{
32 struct zsQueue* q;
33
34 if ((q = (struct zsQueue*)zfwMemAllocate(dev, sizeof(struct zsQueue)
35 + (sizeof(struct zsQueueCell)*(size-1)))) != NULL)
36 {
37 q->size = size;
38 q->sizeMask = size-1;
39 q->head = 0;
40 q->tail = 0;
41 }
42 return q;
43}
44
45void zfQueueDestroy(zdev_t* dev, struct zsQueue* q)
46{
47 u16_t size = sizeof(struct zsQueue) + (sizeof(struct zsQueueCell)*(q->size-1));
48
49 zfQueueFlush(dev, q);
50 zfwMemFree(dev, q, size);
51
52 return;
53}
54
55u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
56{
57 u16_t ret = ZM_ERR_QUEUE_FULL;
58
59 zm_msg0_mm(ZM_LV_1, "zfQueuePutNcs()");
60
61 if (((q->tail+1)&q->sizeMask) != q->head)
62 {
63 q->cell[q->tail].buf = buf;
64 q->cell[q->tail].tick = tick;
65 q->tail = (q->tail+1) & q->sizeMask;
66 ret = ZM_SUCCESS;
67 }
68
69 return ret;
70}
71
72u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
73{
74 u16_t ret;
75 zmw_declare_for_critical_section();
76
77 zmw_enter_critical_section(dev);
78
79 ret = zfQueuePutNcs(dev, q, buf, tick);
80
81 zmw_leave_critical_section(dev);
82
83 return ret;
84}
85
86zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q)
87{
88 zbuf_t* buf = NULL;
89 zmw_declare_for_critical_section();
90
91 zmw_enter_critical_section(dev);
92
93 if (q->head != q->tail)
94 {
95 buf = q->cell[q->head].buf;
96 q->head = (q->head+1) & q->sizeMask;
97 }
98
99 zmw_leave_critical_section(dev);
100
101 return buf;
102}
103
104u16_t zfCompareDstwithBuf(zdev_t* dev, zbuf_t* buf, u8_t* addr)
105{
106 u16_t i;
107 u8_t dst[6];
108
109 for (i=0; i<6; i++)
110 {
111 dst[i] = zmw_buf_readb(dev, buf, i);
112 if (dst[i] != addr[i])
113 {
114 return 1+i;
115 }
116 }
117
118 return 0;
119}
120
121
122zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb)
123{
124 zbuf_t* buf;
125 zbuf_t* retBuf = NULL;
126 u16_t index, next;
127 zmw_declare_for_critical_section();
128
129 *mb = 0;
130
131 zmw_enter_critical_section(dev);
132
133 index = q->head;
134
135 while (1)
136 {
137 if (index != q->tail)
138 {
139 buf = q->cell[index].buf;
140
141
142 if (zfCompareDstwithBuf(dev, buf, addr) == 0)
143 {
144 retBuf = buf;
145
146 while ((next =((index+1)&q->sizeMask)) != q->tail)
147 {
148 q->cell[index].buf = q->cell[next].buf;
149 q->cell[index].tick = q->cell[next].tick;
150
151 if ((*mb == 0) && (zfCompareDstwithBuf(dev,
152 q->cell[next].buf, addr) == 0))
153 {
154 *mb = 1;
155 }
156
157 index = next;
158 }
159 q->tail = (q->tail-1) & q->sizeMask;
160
161 zmw_leave_critical_section(dev);
162 return retBuf;
163 }
164 index = (index + 1) & q->sizeMask;
165 }
166 else
167 {
168 break;
169 }
170 }
171
172 zmw_leave_critical_section(dev);
173
174 return retBuf;
175
176}
177
178void zfQueueFlush(zdev_t* dev, struct zsQueue* q)
179{
180 zbuf_t* buf;
181
182 while ((buf = zfQueueGet(dev, q)) != NULL)
183 {
184 zfwBufFree(dev, buf, 0);
185 }
186
187 return;
188}
189
190void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge)
191{
192 zbuf_t* buf;
193 u32_t buftick;
194 zmw_declare_for_critical_section();
195
196 while (1)
197 {
198 buf = NULL;
199 zmw_enter_critical_section(dev);
200
201 if (q->head != q->tail)
202 {
203 buftick = q->cell[q->head].tick;
204 if (((tick - buftick)*ZM_MS_PER_TICK) > msAge)
205 {
206 buf = q->cell[q->head].buf;
207 q->head = (q->head+1) & q->sizeMask;
208 }
209 }
210
211 zmw_leave_critical_section(dev);
212
213 if (buf != NULL)
214 {
215 zm_msg0_mm(ZM_LV_0, "Age frame in queue!");
216 zfwBufFree(dev, buf, 0);
217 }
218 else
219 {
220 break;
221 }
222 }
223 return;
224}
225
226
227u8_t zfQueueRemovewithIndex(zdev_t* dev, struct zsQueue* q, u16_t index, u8_t* addr)
228{
229 u16_t next;
230 u8_t mb = 0;
231
232
233 while ((next =((index+1)&q->sizeMask)) != q->tail)
234 {
235 q->cell[index].buf = q->cell[next].buf;
236 q->cell[index].tick = q->cell[next].tick;
237
238 if ((mb == 0) && (zfCompareDstwithBuf(dev,
239 q->cell[next].buf, addr) == 0))
240 {
241 mb = 1;
242 }
243
244 index = next;
245 }
246 q->tail = (q->tail-1) & q->sizeMask;
247
248 return mb;
249
250}
251
252void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
253 u8_t* uniBitMap, u16_t* highestByte)
254{
255 zbuf_t* psBuf;
256 u8_t dst[6];
257 u16_t id, aid, index, i;
258 u16_t bitPosition;
259 u16_t bytePosition;
260 zmw_get_wlan_dev(dev);
261 zmw_declare_for_critical_section();
262
263 zmw_enter_critical_section(dev);
264
265 index = q->head;
266
267 while (index != q->tail)
268 {
269 psBuf = q->cell[index].buf;
270 for (i=0; i<6; i++)
271 {
272 dst[i] = zmw_buf_readb(dev, psBuf, i);
273 }
274
275 if (((id = zfApFindSta(dev, (u16_t*)dst)) != 0xffff)
276 && (wd->ap.staTable[id].psMode != 0))
277 {
278
279 if ((wd->ap.staTable[id].qosInfo & 0xf) == 0xf)
280 {
281 aid = id + 1;
282 bitPosition = (1 << (aid & 0x7));
283 bytePosition = (aid >> 3);
284 uniBitMap[bytePosition] |= bitPosition;
285
286 if (bytePosition>*highestByte)
287 {
288 *highestByte = bytePosition;
289 }
290 }
291 index = (index+1) & q->sizeMask;
292 }
293 else
294 {
295
296 zfQueueRemovewithIndex(dev, q, index, dst);
297 zfwBufFree(dev, psBuf, 0);
298 }
299 }
300 zmw_leave_critical_section(dev);
301
302 return;
303}
304