14 #define DEBUG_CMD(_a) if (DEBUG_ON) { _a }
17 #define MIN_CAPACITY 128 /* min allocated buffer for a packet */
19 static int pqueue_alloc (int seq
, unsigned char *packet
, int packlen
, pqueue_t
**new);
21 int packet_timeout_usecs
= DEFAULT_PACKET_TIMEOUT
* 1000000;
24 static pqueue_t
*pq_head
= NULL
, *pq_tail
= NULL
;
26 /* contains a list of free queue elements.*/
27 static pqueue_t
*pq_freelist_head
= NULL
;
31 static int pqueue_alloc(int seq
, unsigned char *packet
, int packlen
, pqueue_t
**new) {
35 DEBUG_CMD(log("seq=%d, packlen=%d", seq
, packlen
););
37 /* search the freelist for one that has sufficient space */
38 if (pq_freelist_head
) {
40 for (newent
= pq_freelist_head
; newent
; newent
= newent
->next
) {
42 if (newent
->capacity
>= packlen
) {
44 /* unlink from freelist */
45 if (pq_freelist_head
== newent
)
46 pq_freelist_head
= newent
->next
;
49 newent
->prev
->next
= newent
->next
;
52 newent
->next
->prev
= newent
->prev
;
55 pq_freelist_head
->prev
= NULL
;
58 } /* end if capacity >= packlen */
61 /* nothing found? Take first and reallocate it */
64 newent
= pq_freelist_head
;
65 pq_freelist_head
= pq_freelist_head
->next
;
68 pq_freelist_head
->prev
= NULL
;
70 DEBUG_CMD(log("realloc capacity %d to %d",newent
->capacity
, packlen
););
72 newent
->packet
= (unsigned char *)realloc(newent
->packet
, packlen
);
74 if (!newent
->packet
) {
75 warn("error reallocating packet: %s", strerror(errno
));
78 newent
->capacity
= packlen
;
81 DEBUG_CMD(log("Recycle entry from freelist. Capacity: %d", newent
->capacity
););
85 /* allocate a new one */
86 newent
= (pqueue_t
*)malloc( sizeof(pqueue_t
) );
88 warn("error allocating newent: %s", strerror(errno
));
93 DEBUG_CMD(log("Alloc new queue entry"););
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
));
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
);
112 newent
->next
= newent
->prev
= NULL
;
114 newent
->packlen
= packlen
;
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;
127 int pqueue_add (int seq
, unsigned char *packet
, int packlen
) {
128 pqueue_t
*newent
, *point
;
130 /* get a new entry */
131 if ( 0 != pqueue_alloc(seq
, packet
, packlen
, &newent
) ) {
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
);
141 if (point
->seq
> seq
) {
142 // gone too far: point->seq > seq and point->prev->seq < seq
144 // insert between point->prev and point
145 DEBUG_CMD(log("adding %d between %d and %d",
146 seq
, point
->prev
->seq
, point
->seq
););
148 point
->prev
->next
= newent
;
150 // insert at head of queue, before point
151 DEBUG_CMD(log("adding %d before %d", seq
, point
->seq
););
154 newent
->prev
= point
->prev
; // will be NULL, at head of queue
155 newent
->next
= point
;
156 point
->prev
= newent
;
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)
167 if (pq_head
== NULL
) {
168 DEBUG_CMD(log("adding %d to empty queue", seq
););
171 DEBUG_CMD(log("adding %d as tail, after %d", seq
, pq_tail
->seq
););
172 pq_tail
->next
= newent
;
174 newent
->prev
= pq_tail
;
182 int pqueue_del (pqueue_t
*point
) {
184 DEBUG_CMD(log("Move seq %d to freelist", point
->seq
););
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
;
192 /* add point to the freelist */
193 point
->next
= pq_freelist_head
;
197 point
->next
->prev
= point
;
198 pq_freelist_head
= point
;
202 int pq_freelist_count
= 0;
204 for ( point
= pq_head
; point
; point
= point
->next
) {
208 for ( point
= pq_freelist_head
; point
; point
= point
->next
) {
211 log("queue length is %d, freelist length is %d", pq_count
, pq_freelist_count
);
219 pqueue_t
*pqueue_head () {
225 int pqueue_expiry_time (pqueue_t
*entry
) {
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
);