release/src/router/pptp-client/pqueue.c

   1 #include <errno.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <assert.h>
   5
   6 #include "util.h" // for log()
   7 #include "pqueue.h"
   8
   9 #ifdef DEBUG_PQUEUE
  10 #define DEBUG_ON 1
  11 #else
  12 #define DEBUG_ON 0
  13 #endif
  14
  15 #define DEBUG_CMD(_a) if (DEBUG_ON) { _a }
  16
  17
  18 #define MIN_CAPACITY 128 /* min allocated buffer for a packet */
  19
  20 static int pqueue_alloc (int seq, unsigned char *packet, int packlen, pqueue_t **new);
  21
  22 int packet_timeout_usecs = DEFAULT_PACKET_TIMEOUT * 1000000;
  23
  24
  25 static pqueue_t *pq_head = NULL, *pq_tail = NULL;
  26
  27 /* contains a list of free queue elements.*/
  28 static pqueue_t *pq_freelist_head = NULL;
  29
  30
  31
  32 static int pqueue_alloc(int seq, unsigned char *packet, int packlen, pqueue_t **new) {
  33
  34   pqueue_t *newent;
  35
  36   DEBUG_CMD(log("seq=%d, packlen=%d", seq, packlen););
  37
  38   /* search the freelist for one that has sufficient space  */
  39   if (pq_freelist_head) {
  40
  41     for (newent = pq_freelist_head; newent; newent = newent->next) {
  42
  43       if (newent->capacity >= packlen) {
  44
  45         /* unlink from freelist */
  46         if (pq_freelist_head == newent)
  47           pq_freelist_head = newent->next;
  48
  49         if (newent->prev)
  50           newent->prev->next = newent->next;
  51
  52         if (newent->next)
  53           newent->next->prev = newent->prev;
  54
  55         if (pq_freelist_head)
  56           pq_freelist_head->prev = NULL;
  57
  58         break;
  59       } /* end if capacity >= packlen */
  60     } /* end for */
  61
  62     /* nothing found? Take first and reallocate it */
  63     if (NULL == newent) {
  64
  65       newent = pq_freelist_head;
  66       pq_freelist_head = pq_freelist_head->next;
  67
  68       if (pq_freelist_head)
  69         pq_freelist_head->prev = NULL;
  70
  71       DEBUG_CMD(log("realloc capacity %d to %d",newent->capacity, packlen););
  72
  73       newent->packet = (unsigned char *)realloc(newent->packet, packlen);
  74
  75       if (!newent->packet) {
  76         warn("error reallocating packet: %s", strerror(errno));
  77         return -1;
  78       }
  79       newent->capacity = packlen;
  80     }
  81
  82     DEBUG_CMD(log("Recycle entry from freelist. Capacity: %d", newent->capacity););
  83
  84   } else {
  85
  86     /* allocate a new one */
  87     newent = (pqueue_t *)malloc( sizeof(pqueue_t) );
  88     if (!newent) {
  89       warn("error allocating newent: %s", strerror(errno));
  90       return -1;
  91     }
  92     newent->capacity = 0;
  93
  94     DEBUG_CMD(log("Alloc new queue entry"););
  95   }
  96
  97   if ( ! newent->capacity  ) {
  98     /* a new queue entry was allocated. Allocate the packet buffer */
  99     int size = packlen < MIN_CAPACITY ? MIN_CAPACITY : packlen;
 100     /* Allocate at least MIN_CAPACITY */
 101     DEBUG_CMD(log("allocating for packet size %d", size););
 102     newent->packet = (unsigned char *)malloc(size);
 103     if (!newent->packet) {
 104       warn("error allocating packet: %s", strerror(errno));
 105       return -1;
 106     }
 107     newent->capacity = size;
 108   } /* endif ! capacity */
 109   assert( newent->capacity >= packlen );
 110   /* store the contents into the buffer */
 111   memcpy(newent->packet, packet, packlen);
 112
 113   newent->next = newent->prev = NULL;
 114   newent->seq     = seq;
 115   newent->packlen = packlen;
 116
 117   gettimeofday(&newent->expires, NULL);
 118   newent->expires.tv_usec += packet_timeout_usecs;
 119   newent->expires.tv_sec  += (newent->expires.tv_usec / 1000000);
 120   newent->expires.tv_usec %= 1000000;
 121
 122   *new = newent;
 123   return 0;
 124 }
 125
 126
 127
 128 int pqueue_add (int seq, unsigned char *packet, int packlen) {
 129   pqueue_t *newent, *point;
 130
 131   /* get a new entry */
 132   if ( 0 != pqueue_alloc(seq, packet, packlen, &newent) ) {
 133     return -1;
 134   }
 135
 136   for (point = pq_head; point != NULL; point = point->next) {
 137     if (point->seq == seq) {
 138       // queue already contains this packet
 139       warn("discarding duplicate packet %d", seq);
 140       return -1;
 141     }
 142     if (point->seq > seq) {
 143       // gone too far: point->seq > seq and point->prev->seq < seq
 144       if (point->prev) {
 145         // insert between point->prev and point
 146         DEBUG_CMD(log("adding %d between %d and %d",
 147                       seq, point->prev->seq, point->seq););
 148
 149         point->prev->next = newent;
 150       } else {
 151         // insert at head of queue, before point
 152         DEBUG_CMD(log("adding %d before %d", seq, point->seq););
 153         pq_head = newent;
 154       }
 155       newent->prev = point->prev; // will be NULL, at head of queue
 156       newent->next = point;
 157       point->prev = newent;
 158       return 0;
 159     }
 160   }
 161
 162   /* We didn't find anywhere to insert the packet,
 163    * so there are no packets in the queue with higher sequences than this one,
 164    * so all the packets in the queue have lower sequences,
 165    * so this packet belongs at the end of the queue (which might be empty)
 166    */
 167
 168   if (pq_head == NULL) {
 169     DEBUG_CMD(log("adding %d to empty queue", seq););
 170     pq_head = newent;
 171   } else {
 172     DEBUG_CMD(log("adding %d as tail, after %d", seq, pq_tail->seq););
 173     pq_tail->next = newent;
 174   }
 175   newent->prev = pq_tail;
 176   pq_tail = newent;
 177
 178   return 0;
 179 }
 180
 181
 182
 183 int pqueue_del (pqueue_t *point) {
 184
 185   DEBUG_CMD(log("Move seq %d to freelist", point->seq););
 186
 187   /* unlink from pq */
 188   if (pq_head == point) pq_head = point->next;
 189   if (pq_tail == point) pq_tail = point->prev;
 190   if (point->prev) point->prev->next = point->next;
 191   if (point->next) point->next->prev = point->prev;
 192
 193   /* add point to the freelist */
 194   point->next = pq_freelist_head;
 195   point->prev = NULL;
 196
 197   if (point->next)
 198     point->next->prev = point;
 199   pq_freelist_head = point;
 200
 201   DEBUG_CMD(
 202     int pq_count = 0;
 203     int pq_freelist_count = 0;
 204     pqueue_t *point;
 205     for ( point = pq_head; point ; point = point->next) {
 206       ++pq_count;
 207     }
 208
 209     for ( point = pq_freelist_head; point ; point = point->next) {
 210       ++pq_freelist_count;
 211     }
 212     log("queue length is %d, freelist length is %d", pq_count, pq_freelist_count);
 213     );
 214
 215   return 0;
 216 }
 217
 218
 219
 220 pqueue_t *pqueue_head () {
 221   return pq_head;
 222 }
 223
 224
 225
 226 int pqueue_expiry_time (pqueue_t *entry) {
 227   struct timeval tv;
 228   int expiry_time;
 229
 230   gettimeofday(&tv, NULL);
 231   expiry_time  = (entry->expires.tv_sec  - tv.tv_sec) * 1000000;
 232   expiry_time += (entry->expires.tv_usec - tv.tv_usec);
 233   return expiry_time;
 234 }