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     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
  45 void 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
  55 u16_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
  72 u16_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
  86 zbuf_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
 104 u16_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
 122 zbuf_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             //if buf's detination address == input addr
 142             if (zfCompareDstwithBuf(dev, buf, addr) == 0)
 143             {
 144                 retBuf = buf;
 145                 //Get it, and trace the whole queue to calculate more bit
 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         } //if (index != q->tail)
 166         else
 167         {
 168             break;
 169         }
 170     }
 171
 172     zmw_leave_critical_section(dev);
 173
 174     return retBuf;
 175
 176 }
 177
 178 void 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
 190 void 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
 227 u8_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     //trace the whole queue to calculate more bit
 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
 252 void 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         /* TODO : use u8_t* fot MAC address */
 275         if (((id = zfApFindSta(dev, (u16_t*)dst)) != 0xffff)
 276                 && (wd->ap.staTable[id].psMode != 0))
 277         {
 278             /* Calculate PVB only when all AC are delivery-enabled */
 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             /* Free garbage UAPSD frame */
 296             zfQueueRemovewithIndex(dev, q, index, dst);
 297             zfwBufFree(dev, psBuf, 0);
 298         }
 299     }
 300     zmw_leave_critical_section(dev);
 301
 302     return;
 303 }