1 files changed, 291 insertions, 0 deletions
diff --git a/gtp/queue.c b/gtp/queue.c
new file mode 100644
index 0000000..ebeebe6
--- /dev/null
+++ b/gtp/queue.c
@@ -0,0 +1,291 @@
+/* 
+ *  OsmoGGSN - Gateway GPRS Support Node
+ *  Copyright (C) 2002, 2003, 2004 Mondru AB.
+ *  Copyright (C) 2011 Harald Welte <laforge@gnumonks.org>
+ *  Copyright (C) 2016 sysmocom - s.f.m.c. GmbH
+ * 
+ *  The contents of this file may be used under the terms of the GNU
+ *  General Public License Version 2, provided that the above copyright
+ *  notice and this permission notice is included in all copies or
+ *  substantial portions of the software.
+ * 
+ */
+
+/*
+ * Queue.c
+ * Reliable delivery of signalling messages
+ */
+
+#include <../config.h>
+#ifdef HAVE_STDINT_H
+#include <stdint.h>
+#endif
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <netinet/in.h>
+#include <string.h>
+#include "pdp.h"
+#include "gtp.h"
+#include "queue.h"
+
+/*! \brief dump a queue_t to stdout */
+static int queue_print(struct queue_t *queue)
+{
+	int n;
+	printf("Queue: %p Next: %d First: %d Last: %d\n", queue,
+	       queue->next, queue->first, queue->last);
+	printf("# State seq next prev timeout retrans\n");
+	for (n = 0; n < QUEUE_SIZE; n++) {
+		printf("%d %d %d %d %d %d %d\n",
+		       n,
+		       queue->qmsga[n].state,
+		       queue->qmsga[n].seq,
+		       queue->qmsga[n].next,
+		       queue->qmsga[n].prev,
+		       (int)queue->qmsga[n].timeout, queue->qmsga[n].retrans);
+	}
+	return 0;
+}
+
+/*! \brief compute the hash function */
+static int queue_seqhash(struct sockaddr_in *peer, uint16_t seq)
+{
+	/* With QUEUE_HASH_SIZE = 2^16 this describes all possible
+	   seq values. Thus we have perfect hash for the request queue.
+	   For the response queue we might have collisions, but not very
+	   often.
+	   For performance optimisation we should remove the modulus
+	   operator, but this is only valid for QUEUE_HASH_SIZE = 2^16 */
+	return seq % QUEUE_HASH_SIZE;
+}
+
+/*! \brief Insert a message with given sequence number into the hash
+ *
+ * This function sets the peer and the seq of the qmsg and then inserts
+ * the qmsg into the queue hash.  To do so, it does a hashtable lookup
+ * and appends the new entry as the last into the double-linked list of
+ * entries for this sequence number.
+ */
+static int queue_seqset(struct queue_t *queue, struct qmsg_t *qmsg,
+		 	struct sockaddr_in *peer, uint16_t seq)
+{
+	int hash = queue_seqhash(peer, seq);
+	struct qmsg_t *qmsg2;
+	struct qmsg_t *qmsg_prev = NULL;
+
+	if (QUEUE_DEBUG)
+		printf("Begin queue_seqset seq = %d\n", (int)seq);
+	if (QUEUE_DEBUG)
+		printf("SIZEOF PEER %zu, *PEER %zu\n", sizeof(peer),
+		       sizeof(*peer));
+
+	qmsg->seq = seq;
+	memcpy(&qmsg->peer, peer, sizeof(*peer));
+
+	for (qmsg2 = queue->hashseq[hash]; qmsg2; qmsg2 = qmsg2->seqnext)
+		qmsg_prev = qmsg2;
+	if (!qmsg_prev)
+		queue->hashseq[hash] = qmsg;
+	else
+		qmsg_prev->seqnext = qmsg;
+	if (QUEUE_DEBUG)
+		printf("End queue_seqset\n");
+	return 0;
+}
+
+/*! \brief Remove a given qmsg_t from the queue hash */
+static int queue_seqdel(struct queue_t *queue, struct qmsg_t *qmsg)
+{
+	int hash = queue_seqhash(&qmsg->peer, qmsg->seq);
+	struct qmsg_t *qmsg2;
+	struct qmsg_t *qmsg_prev = NULL;
+	if (QUEUE_DEBUG)
+		printf("Begin queue_seqdel seq = %d\n", (int)qmsg->seq);
+
+	for (qmsg2 = queue->hashseq[hash]; qmsg2; qmsg2 = qmsg2->seqnext) {
+		if (qmsg == qmsg2) {
+			if (!qmsg_prev)
+				queue->hashseq[hash] = qmsg2->seqnext;
+			else
+				qmsg_prev->seqnext = qmsg2->seqnext;
+			if (QUEUE_DEBUG)
+				printf("End queue_seqdel: SEQ found\n");
+			return 0;
+		}
+		qmsg_prev = qmsg2;
+	}
+	printf("End queue_seqdel: SEQ not found\n");
+	return EOF;		/* End of linked list and not found */
+}
+
+/*! \brief Allocates and initialises new queue structure */
+int queue_new(struct queue_t **queue)
+{
+	if (QUEUE_DEBUG)
+		printf("queue_new\n");
+	*queue = calloc(1, sizeof(struct queue_t));
+	if (!(*queue))
+		return EOF;
+	(*queue)->next = 0;
+	(*queue)->first = -1;
+	(*queue)->last = -1;
+
+	if (QUEUE_DEBUG)
+		queue_print(*queue);
+	return 0;
+}
+
+/*! \brief Deallocates queue structure */
+int queue_free(struct queue_t *queue)
+{
+	if (QUEUE_DEBUG)
+		printf("queue_free\n");
+	if (QUEUE_DEBUG)
+		queue_print(queue);
+	free(queue);
+	return 0;
+}
+
+/*! \brief Add a new message to the queue */
+int queue_newmsg(struct queue_t *queue, struct qmsg_t **qmsg,
+		 struct sockaddr_in *peer, uint16_t seq)
+{
+	if (QUEUE_DEBUG)
+		printf("queue_newmsg %d\n", (int)seq);
+	if (queue->qmsga[queue->next].state == 1) {
+		return EOF;	/* Queue is full */
+	} else {
+		*qmsg = &queue->qmsga[queue->next];
+		queue_seqset(queue, *qmsg, peer, seq);
+		(*qmsg)->state = 1;	/* Space taken */
+		(*qmsg)->this = queue->next;
+		(*qmsg)->next = -1;	/* End of the queue */
+		(*qmsg)->prev = queue->last;	/* Link to the previous */
+		if (queue->last != -1)
+			queue->qmsga[queue->last].next = queue->next;	/* Link previous to us */
+		queue->last = queue->next;	/* End of queue */
+		if (queue->first == -1)
+			queue->first = queue->next;
+		queue->next = (queue->next + 1) % QUEUE_SIZE;	/* Increment */
+		if (QUEUE_DEBUG)
+			queue_print(queue);
+		return 0;
+	}
+}
+
+/*! \brief Simply remoev a given qmsg_t from the queue
+ *
+ * Internally, we first delete the entry from the queue, and then update
+ * up our global queue->first / queue->last pointers.  Finally,
+ * the qmsg_t is re-initialized with zero bytes.  No memory is released.
+ */
+int queue_freemsg(struct queue_t *queue, struct qmsg_t *qmsg)
+{
+	if (QUEUE_DEBUG)
+		printf("queue_freemsg\n");
+	if (qmsg->state != 1) {
+		return EOF;	/* Not in queue */
+	}
+
+	queue_seqdel(queue, qmsg);
+
+	if (qmsg->next == -1)	/* Are we the last in queue? */
+		queue->last = qmsg->prev;
+	else
+		queue->qmsga[qmsg->next].prev = qmsg->prev;
+
+	if (qmsg->prev == -1)	/* Are we the first in queue? */
+		queue->first = qmsg->next;
+	else
+		queue->qmsga[qmsg->prev].next = qmsg->next;
+
+	memset(qmsg, 0, sizeof(struct qmsg_t));	/* Just to be safe */
+
+	if (QUEUE_DEBUG)
+		queue_print(queue);
+
+	return 0;
+}
+
+/*! \brief Move a given qmsg_t to the end of the queue ?!? */
+int queue_back(struct queue_t *queue, struct qmsg_t *qmsg)
+{
+	if (QUEUE_DEBUG)
+		printf("queue_back\n");
+	if (qmsg->state != 1) {
+		return EOF;	/* Not in queue */
+	}
+
+	/* Insert stuff to maintain hash table */
+
+	if (qmsg->next != -1) {	/* Only swop if there are others */
+		queue->qmsga[qmsg->next].prev = qmsg->prev;
+		queue->first = qmsg->next;
+
+		qmsg->next = -1;
+		qmsg->prev = queue->last;
+		if (queue->last != -1)
+			queue->qmsga[queue->last].next = qmsg->this;
+		queue->last = qmsg->this;
+	}
+	if (QUEUE_DEBUG)
+		queue_print(queue);
+	return 0;
+}
+
+/*! \brief Get the first element in the entire queue */
+int queue_getfirst(struct queue_t *queue, struct qmsg_t **qmsg)
+{
+	/*printf("queue_getfirst\n"); */
+	if (queue->first == -1) {
+		*qmsg = NULL;
+		return EOF;	/* End of queue = queue is empty. */
+	}
+	*qmsg = &queue->qmsga[queue->first];
+	if (QUEUE_DEBUG)
+		queue_print(queue);
+	return 0;
+}
+
+/*! \brief Get a queue entry for a given peer + seq */
+int queue_seqget(struct queue_t *queue, struct qmsg_t **qmsg,
+		 struct sockaddr_in *peer, uint16_t seq)
+{
+	int hash = queue_seqhash(peer, seq);
+	struct qmsg_t *qmsg2;
+	if (QUEUE_DEBUG)
+		printf("Begin queue_seqget seq = %d\n", (int)seq);
+	for (qmsg2 = queue->hashseq[hash]; qmsg2; qmsg2 = qmsg2->seqnext) {
+		if ((qmsg2->seq == seq) &&
+		    (!memcmp(&qmsg2->peer, peer, sizeof(*peer)))) {
+			*qmsg = qmsg2;
+			if (QUEUE_DEBUG)
+				printf("End queue_seqget. Found\n");
+			return 0;
+		}
+	}
+	if (QUEUE_DEBUG)
+		printf("End queue_seqget. Not found\n");
+	return EOF;		/* End of linked list and not found */
+}
+
+/*! \brief look-up a given seq/peer, return cbp + type and free entry */
+int queue_freemsg_seq(struct queue_t *queue, struct sockaddr_in *peer,
+		      uint16_t seq, uint8_t * type, void **cbp)
+{
+	struct qmsg_t *qmsg;
+	if (queue_seqget(queue, &qmsg, peer, seq)) {
+		*cbp = NULL;
+		*type = 0;
+		return EOF;
+	}
+	*cbp = qmsg->cbp;
+	*type = qmsg->type;
+	if (queue_freemsg(queue, qmsg)) {
+		return EOF;
+	}
+	return 0;
+}