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 20static int pqueue_alloc (int seq, unsigned char *packet, int packlen, pqueue_t **new); 21 22int packet_timeout_usecs = DEFAULT_PACKET_TIMEOUT * 1000000; 23 24 25static pqueue_t *pq_head = NULL, *pq_tail = NULL; 26 27/* contains a list of free queue elements.*/ 28static pqueue_t *pq_freelist_head = NULL; 29 30 31 32static 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 128int 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 183int 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 220pqueue_t *pqueue_head () { 221 return pq_head; 222} 223 224 225 226int 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} 235