drivers/staging/otus/80211core/queue.c

   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
  34     q = (struct zsQueue*)zfwMemAllocate(dev, sizeof(struct zsQueue)
  35             + (sizeof(struct zsQueueCell)*(size-1)));
  36     if (q != NULL)
  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 }