332 lines
9.3 KiB
C
332 lines
9.3 KiB
C
/*
|
|
* 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 */
|
|
}
|
|
|
|
/*! Allocates and initialises new queue structure.
|
|
* \param[out] queue pointer where to store the allocated object. Must be freed with queue_free
|
|
* \returns zero on success, non-zero on error
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/*! Deallocates queue structure.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \returns zero on success, non-zero on error.
|
|
*/
|
|
int queue_free(struct queue_t *queue)
|
|
{
|
|
if (QUEUE_DEBUG)
|
|
printf("queue_free\n");
|
|
if (QUEUE_DEBUG)
|
|
queue_print(queue);
|
|
free(queue);
|
|
return 0;
|
|
}
|
|
|
|
/*! Add a new message to the queue.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \param[out] qmsg first message from the queue (if succeeds)
|
|
* \param[in] peer who sent the message to add
|
|
* \param[in] seq sequence number of the message to add
|
|
* \returns zero on success, non-zero on error.
|
|
*/
|
|
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);
|
|
INIT_LLIST_HEAD(&(*qmsg)->entry);
|
|
(*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;
|
|
}
|
|
}
|
|
|
|
|
|
/*! Remove an element from the queue.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \param[in] qmsg message to free
|
|
* \returns zero on success, non-zero on error.
|
|
*
|
|
* 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 */
|
|
}
|
|
|
|
llist_del(&qmsg->entry);
|
|
|
|
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;
|
|
}
|
|
|
|
/*! Move a given qmsg_t to the end of the queue.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \param[in] qmsg message to move to the end of the queue
|
|
* \returns zero on success, non-zero on error.
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/*! Get the first element in the entire queue.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \param[out] qmsg first message from the queue (if succeeds)
|
|
* \returns zero on success, non-zero on error.
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/*! Get a queue entry for a given peer + seq.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \param[out] qmsg first message from the queue (if succeeds)
|
|
* \param[in] peer who sent the message to retrieve
|
|
* \param[in] seq sequence number of the message to retrive
|
|
* \returns zero on success, non-zero on error.
|
|
*/
|
|
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 */
|
|
}
|
|
|
|
/*! look-up a given seq/peer, return cbp + type and free entry.
|
|
* \param[in] queue pointer previously allocated by queue_new
|
|
* \param[in] peer who sent the message to retrieve
|
|
* \param[in] seq sequence number of the message to retrive
|
|
* \param[out] type GTP message type
|
|
* \param[out] type callback pointer of the message
|
|
* \returns zero on success, non-zero on error.
|
|
*/
|
|
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;
|
|
}
|