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