drivers/usb/net/Zydas/zdlpmgt.c

   1 #if ZDCONF_LP_SUPPORT == 1
   2 #include "zd1205.h"
   3 #include "zdlpmgt.h"
   4
   5 static struct lp_queue lp_q_sending;
   6 static struct lp_queue lp_q_empty;
   7 static struct lp_queue lp_q_half;
   8 static struct lp_desc lp_bucket[LP_BUCKETS];
   9 static spinlock_t lp_spinlock;
  10 static U32 lp_spinlock_flag;
  11 static U32 varLP_MAX_PKTS_IN_BUCKET = LP_MAX_PKTS_IN_BUCKET;
  12 //static U32 varLP_MAX_SIZE_IN_BUCKET = LP_MAX_SIZE_IN_BUCKET;
  13 static U32 varLP_TIMEOUT = LP_TIMEOUT;
  14 static U16 LP_MAX_SIZE_IN_BUCKET[]= { 3839,1792 };
  15
  16
  17 U32 LP_CNT_PUSH_SUCC;
  18 U32 LP_CNT_PUSH_FAIL;
  19 U32 LP_CNT_POP_SUCC;
  20 U32 LP_CNT_POP_FAIL;
  21 U32 LP_CNT_PERIOD_POP_SUCC;
  22 U32 LP_CNT_PERIOD_POP_FAIL;
  23 U32 LP_CNT_LAST_NEW_BUCK_IDX;
  24 U32 LP_CNT_LAST_LATENCY;
  25
  26 void lp_lock(void)
  27 {
  28         if(!in_irq())spin_lock_irqsave(&lp_spinlock, lp_spinlock_flag);
  29 }
  30 void lp_unlock(void)
  31 {
  32         if(!in_irq())spin_unlock_irqrestore(&lp_spinlock, lp_spinlock_flag);
  33 }
  34 void setLP_MAX_PKTS_IN_BUCKET(U32 pkts)
  35 {
  36         lp_lock();
  37         varLP_MAX_PKTS_IN_BUCKET = pkts;
  38         lp_unlock();
  39 }
  40 void setLP_TIMEOUT(U32 uSec)
  41 {
  42     lp_lock();
  43     varLP_TIMEOUT= uSec;
  44     lp_unlock();
  45
  46 }
  47 static struct lp_desc * LP_pop_lp_desc(struct lp_queue *lpq)
  48 {
  49     struct lp_desc *element;
  50     if(lpq == NULL )
  51     {
  52         printk("LP_pop_lp_desc has NULL argument\n");
  53         return NULL;
  54     }
  55     if(lpq->first == NULL)
  56     {
  57         return NULL;
  58     }
  59     element = lpq->first;
  60     lpq->first = element->next;
  61     if(NULL == lpq->first) lpq->last = NULL;
  62     element->next = NULL;
  63     lpq->count--;
  64     if(lpq->count < 0) printk("!!!!!!!! lpq->count < 0\n");
  65     return element;
  66 }
  67 static struct lp_desc * LP_midpop_lp_desc(struct lp_queue *lpq, struct lp_desc *desc)
  68 {
  69     struct lp_desc *element;
  70     struct lp_desc *last_desc = NULL;
  71     U16 loopCheck = 0;
  72     if(lpq == NULL  || desc == NULL)
  73     {
  74         printk("LP_midpop_lp_desc has NULL argument\n");
  75         return NULL;
  76     }
  77     if(lpq->first == NULL)
  78     {
  79         return NULL;
  80     }
  81     element = lpq->first;
  82     while(element)
  83     {
  84         if(loopCheck++ > 100)
  85         {
  86             printk("infinite loop occurs in %s\n", __FUNCTION__);
  87             loopCheck = 0;
  88             break;
  89         }
  90
  91         if(element == desc)
  92         {
  93             if(element == lpq->last) lpq->last = last_desc;
  94             if(!last_desc) //desc is lpq->first
  95             {
  96                 lpq->first = element->next;
  97             }
  98             else
  99                 last_desc->next = element->next;
 100             lpq->count --;
 101             element->next = NULL;
 102             return element;
 103         }
 104         last_desc = element;
 105         element = element->next;
 106     }
 107     return NULL;
 108 }
 109
 110 static void LP_push_lp_desc(struct lp_queue *lpq, struct lp_desc *desc)
 111 {
 112     struct lp_desc *element;
 113     if(lpq == NULL || desc == NULL)
 114     {
 115         printk("LP_push_lp_desc has NULL argument\n");
 116         return;
 117     }
 118     if(lpq->first == NULL)
 119     {
 120         lpq->first = desc;
 121         lpq->last = desc;
 122         desc->next = NULL;
 123         lpq->count = 1;
 124         return;
 125     }
 126     element = lpq->last;
 127     lpq->last = desc;
 128     element->next = desc;
 129     desc->next = NULL;
 130     lpq->count++;
 131     return;
 132 }
 133 static struct lp_desc * LP_first_lp_desc(struct lp_queue *lpq)
 134 {
 135     if(lpq == NULL )
 136     {
 137         printk("LP_push_lp_desc has NULL argument\n");
 138         return NULL;
 139     }
 140     return lpq->first;
 141 }
 142 /*
 143 int LP_totalReadyLen(U32 NOW) {
 144     int i,total=0;
 145     for(i=0;i<LP_BUCKETS;i++) {
 146         if(lp_bucket[i].sending == 1) continue;
 147         if(lp_bucket[i].pktCnt == 0) continue;
 148         if((lp_bucket[i].pktCnt >= varLP_MAX_PKTS_IN_BUCKET) ||
 149            (lp_bucket[i].pktSize > varLP_MAX_SIZE_IN_BUCKET*8/10) ||
 150            ((NOW - lp_bucket[i].createTime) >= varLP_TIMEOUT ))
 151         {
 152             total+= lp_bucket[i].pktSize;
 153         }
 154
 155     }
 156     return total;
 157 }
 158 */
 159 void lp_recycle_tx_bucket(struct lp_desc * bucket)
 160 {
 161     lp_lock();
 162     if(NULL == bucket)
 163     {
 164         printk("Why do you recycle NULL bucket\n");
 165         lp_unlock();
 166         return;
 167     }
 168     if(!LP_midpop_lp_desc(&lp_q_sending, bucket))
 169     {
 170         printk("%s, bucket doesn't exists\n", __FUNCTION__);
 171     }
 172     LP_push_lp_desc(&lp_q_empty, bucket);
 173     lp_unlock();
 174     return;
 175 }
 176
 177 int pushPkt(fragInfo_t *pkt, BOOLEAN LenType, U32 NOW)
 178 {
 179         u32 ret=-1;
 180     struct lp_desc *desc;
 181     U16 loopCheck = 0;
 182     if(LenType & ~BIT_0)
 183     {
 184         printk("LenType != 0 or 1, Set it as 1\n");
 185         LenType = 1;
 186     }
 187         lp_lock();
 188     desc = LP_first_lp_desc(&lp_q_half);
 189     while(desc)
 190     {
 191         if(loopCheck++ > 100)
 192         {
 193             printk("infinite loop occurs in %s\n", __FUNCTION__);
 194             loopCheck = 0;
 195             break;
 196         }
 197
 198                 if(desc->sending == 1 ) {
 199             printk("Half is sending !?\n");
 200             desc = desc->next;
 201                         continue;
 202                 }
 203                 if(desc->pktCnt > 0 && desc->pktCnt <  LP_MAX_PKTS_IN_BUCKET && desc->pktSize+pkt->bodyLen[0] < LP_MAX_SIZE_IN_BUCKET[LenType])
 204         {
 205                         if(memcmp(desc->pkt[0].EthHdr, pkt->EthHdr, ETH_ALEN) == 0)
 206             {
 207                                 //printk("Put packet in existing bucket\n ");
 208                                 memcpy(&desc->pkt[desc->pktCnt],pkt,sizeof(fragInfo_t));
 209                                 desc->pktCnt++;
 210                                 desc->pktSize+= pkt->bodyLen[0];
 211                                 lp_unlock();
 212                                 ret = 0;
 213                                 return ret;
 214                         }
 215
 216                 }
 217         desc = desc->next;
 218         }
 219
 220         if(!desc) {
 221
 222                 //printk("Create a new bucket\n");
 223                 //LP_CNT_LAST_NEW_BUCK_IDX = freeBucket;
 224         desc = LP_pop_lp_desc(&lp_q_empty);
 225         if(desc == NULL)
 226         {
 227             printk("We are out of lp_q_empty\n");
 228         }
 229         else
 230         {
 231             memcpy(&desc->pkt[0],pkt,sizeof(fragInfo_t));
 232             desc->pktCnt++;
 233             desc->pktSize = pkt->bodyLen[0];
 234             desc->createTime = NOW;
 235             LP_push_lp_desc(&lp_q_half, desc);
 236             ret = 0;
 237         }
 238     }
 239     lp_unlock();
 240         return ret;
 241 }
 242 struct lp_desc *popPkt(BOOLEAN ANYONE, BOOLEAN LenType, U32 NOW)
 243 {
 244     struct lp_desc *timeoutBucket=NULL;
 245     struct lp_desc *desc;
 246     U16 loopCheck = 0;
 247     if(LenType & ~BIT_0)
 248     {
 249         printk("LenType != 0 or 1, Set it as 1\n");
 250         LenType = 1;
 251     }
 252
 253         lp_lock();
 254     desc = LP_first_lp_desc(&lp_q_half);
 255     if(NULL == desc) return NULL;
 256     while(desc)
 257     {
 258         if(loopCheck++ > 100)
 259         {
 260             printk("infinite loop occurs in %s\n", __FUNCTION__);
 261             loopCheck = 0;
 262             break;
 263         }
 264
 265                 if(desc->sending == 1) {
 266             printk("Wow ~~~ lp_q_half has a sending one\n");
 267             desc = desc->next;
 268                         continue;
 269                 }
 270                 if(desc->pktSize > LP_MAX_SIZE_IN_BUCKET[LenType]*8/10) {
 271             //printk("popPkt an exceed Size LP : %d\n", i);
 272             desc->sending = 1;
 273             if(!LP_midpop_lp_desc(&lp_q_half, desc))
 274                 printk("What !? midpop is NULL ?\n");
 275             LP_push_lp_desc(&lp_q_sending, desc);
 276             lp_unlock();
 277                         LP_CNT_LAST_LATENCY = NOW - desc->createTime;
 278             return desc;
 279                 }
 280                 else if(desc->pktCnt >= varLP_MAX_PKTS_IN_BUCKET) {
 281                         //printk("popPkt an Full LP : %d\n", i);
 282                         desc->sending = 1;
 283             if(!LP_midpop_lp_desc(&lp_q_half, desc))
 284                 printk("What !? midpop is NULL ?\n");
 285             LP_push_lp_desc(&lp_q_sending, desc);
 286                         lp_unlock();
 287                         LP_CNT_LAST_LATENCY = NOW - desc->createTime;
 288                         return desc;
 289                 }
 290                 if((NOW - desc->createTime) >= varLP_TIMEOUT ) {
 291                         if(desc->pktCnt > 0) {
 292                                 //printk("Level 2\n");
 293                                 timeoutBucket = desc;
 294                         }
 295                 }
 296                 else if(ANYONE && desc->pktCnt > 0) {
 297                         desc->sending = 1;
 298             if(!LP_midpop_lp_desc(&lp_q_half, desc))
 299                 printk("What !? midpop is NULL ?\n");
 300             LP_push_lp_desc(&lp_q_sending, desc);
 301
 302                         lp_unlock();
 303                         LP_CNT_LAST_LATENCY = NOW - desc->createTime;
 304                         return desc;
 305                 }
 306         desc = desc->next;
 307         }
 308         if(timeoutBucket) {
 309                 //printk("popPkt an timeout LP : %d\n", timeoutBucket);
 310                 timeoutBucket->sending = 1;
 311         if(!LP_midpop_lp_desc(&lp_q_half, timeoutBucket))
 312             printk("What !? midpop is NULL ?\n");
 313         LP_push_lp_desc(&lp_q_sending, timeoutBucket);
 314
 315                 lp_unlock();
 316                 LP_CNT_LAST_LATENCY = NOW - timeoutBucket->createTime;
 317                 return timeoutBucket;
 318         }
 319         lp_unlock();
 320         return NULL;
 321 }
 322 void LP_CNT_SHOW()
 323 {
 324         printk(KERN_ERR "LP_CNT_PUSH_SUCC:%ld\n",LP_CNT_PUSH_SUCC);
 325         printk(KERN_ERR "LP_CNT_PUSH_FAIL:%ld\n",LP_CNT_PUSH_FAIL);
 326         printk(KERN_ERR "LP_CNT_POP_SUCC:%ld\n",LP_CNT_POP_SUCC);
 327         printk(KERN_ERR "LP_CNT_POP_FAIL:%ld\n",LP_CNT_POP_FAIL);
 328         printk(KERN_ERR "LP_CNT_PERIOD_POP_SUCC:%ld\n",LP_CNT_PERIOD_POP_SUCC);
 329         printk(KERN_ERR "LP_CNT_PERIOD_POP_FAIL:%ld\n",LP_CNT_PERIOD_POP_FAIL);
 330         printk(KERN_ERR "LP_CNT_LAST_NEW_BUCK_IDX:%ld\n",LP_CNT_LAST_NEW_BUCK_IDX);
 331         printk(KERN_ERR "LP_CNT_LAST_LATENCY:%ld\n", LP_CNT_LAST_LATENCY);
 332     printk(KERN_ERR "lp_q_empty cnt : %ld\n", lp_q_empty.count);
 333     printk(KERN_ERR "lp_q_half cnt : %ld\n", lp_q_half.count);
 334     printk(KERN_ERR "lp_q_sending cnt : %ld\n", lp_q_sending.count);
 335 }
 336 void initLP()
 337 {
 338     int i;
 339         memset(lp_bucket,0,sizeof(struct lp_desc)* LP_BUCKETS);
 340         spin_lock_init(&lp_spinlock);
 341     lp_q_sending.first = NULL;
 342     lp_q_sending.count = 0;
 343     lp_q_sending.last = NULL;
 344
 345     lp_q_empty.first = NULL;
 346     lp_q_empty.last = NULL;
 347     lp_q_empty.count = 0;
 348
 349     lp_q_half.first = NULL;
 350     lp_q_half.last = NULL;
 351     lp_q_half.count = 0;
 352
 353     for(i=0;i<LP_BUCKETS;i++)
 354         LP_push_lp_desc(&lp_q_empty, &lp_bucket[i]);
 355         LP_CNT_PUSH_SUCC = 0;
 356         LP_CNT_PUSH_FAIL = 0;
 357         LP_CNT_POP_SUCC = 0;
 358         LP_CNT_POP_FAIL = 0;
 359         LP_CNT_PERIOD_POP_SUCC = 0;
 360         LP_CNT_PERIOD_POP_FAIL = 0;
 361         LP_CNT_LAST_NEW_BUCK_IDX = 0;
 362         LP_CNT_LAST_LATENCY = 0;
 363 }
 364 #endif
 365