release/src/router/pptpd/pqueue.c

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