/*
 *  OsmoGGSN - Gateway GPRS Support Node
 *  Copyright (C) 2002, 2003, 2004 Mondru AB.
 *  Copyright (C) 2010-2011, 2016-2017 Harald Welte <laforge@gnumonks.org>
 *  Copyright (C) 2015-2017 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.
 *
 */

/*
 * gtp.c: Contains all GTP functionality. Should be able to handle multiple
 * tunnels in the same program.
 *
 * TODO:
 *  - Do we need to handle fragmentation?
 */

#ifdef __linux__
#define _GNU_SOURCE 1
#endif

#include <osmocom/core/logging.h>
#include <osmocom/core/utils.h>

#if defined(__FreeBSD__)
#include <sys/endian.h>
#endif

#include "../config.h"
#ifdef HAVE_STDINT_H
#include <stdint.h>
#endif

#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <inttypes.h>

#include <arpa/inet.h>

/* #include <stdint.h>  ISO C99 types */

#include "pdp.h"
#include "gtp.h"
#include "gtpie.h"
#include "queue.h"

/* According to section 14.2 of 3GPP TS 29.006 version 6.9.0 */
#define N3_REQUESTS	5

#define T3_REQUEST	3

/* Error reporting functions */

#define GTP_LOGPKG(pri, peer, pack, len, fmt, args...)			\
	logp2(DLGTP, pri, __FILE__, __LINE__, 0,			\
		"Packet from %s:%u, length: %d content: %s: " fmt,	\
		inet_ntoa((peer)->sin_addr), htons((peer)->sin_port),	\
		len, osmo_hexdump((const uint8_t *) pack, len),		\
		##args);

#define LOGP_WITH_ADDR(ss, level, addr, fmt, args...)                    \
		LOGP(ss, level, "addr(%s:%d) " fmt,                      \
		     inet_ntoa((addr).sin_addr), htons((addr).sin_port), \
		     ##args);

/* API Functions */

const char *gtp_version()
{
	return VERSION;
}

const struct value_string gtp_type_names[] = {
	{ GTP_ECHO_REQ,        "Echo Request" },
	{ GTP_ECHO_RSP,        "Echo Response" },
	{ GTP_NOT_SUPPORTED,   "Version Not Supported" },
	{ GTP_ALIVE_REQ,       "Node Alive Request" },
	{ GTP_ALIVE_RSP,       "Node Alive Response" },
	{ GTP_REDIR_REQ,       "Redirection Request" },
	{ GTP_REDIR_RSP,       "Redirection Response" },
	{ GTP_CREATE_PDP_REQ,  "Create PDP Context Request" },
	{ GTP_CREATE_PDP_RSP,  "Create PDP Context Response" },
	{ GTP_UPDATE_PDP_REQ,  "Update PDP Context Request" },
	{ GTP_UPDATE_PDP_RSP,  "Update PDP Context Response" },
	{ GTP_DELETE_PDP_REQ,  "Delete PDP Context Request" },
	{ GTP_DELETE_PDP_RSP,  "Delete PDP Context Response" },
	{ GTP_ERROR,           "Error Indication" },
	{ GTP_PDU_NOT_REQ,     "PDU Notification Request" },
	{ GTP_PDU_NOT_RSP,     "PDU Notification Response" },
	{ GTP_PDU_NOT_REJ_REQ, "PDU Notification Reject Request" },
	{ GTP_PDU_NOT_REJ_RSP, "PDU Notification Reject Response" },
	{ GTP_SUPP_EXT_HEADER, "Supported Extension Headers Notification" },
	{ GTP_SND_ROUTE_REQ,   "Send Routeing Information for GPRS Request" },
	{ GTP_SND_ROUTE_RSP,   "Send Routeing Information for GPRS Response" },
	{ GTP_FAILURE_REQ,     "Failure Report Request" },
	{ GTP_FAILURE_RSP,     "Failure Report Response" },
	{ GTP_MS_PRESENT_REQ,  "Note MS GPRS Present Request" },
	{ GTP_MS_PRESENT_RSP,  "Note MS GPRS Present Response" },
	{ GTP_IDEN_REQ,        "Identification Request" },
	{ GTP_IDEN_RSP,        "Identification Response" },
	{ GTP_SGSN_CONTEXT_REQ,"SGSN Context Request" },
	{ GTP_SGSN_CONTEXT_RSP,"SGSN Context Response" },
	{ GTP_SGSN_CONTEXT_ACK,"SGSN Context Acknowledge" },
	{ GTP_FWD_RELOC_REQ,   "Forward Relocation Request" },
	{ GTP_FWD_RELOC_RSP,   "Forward Relocation Response" },
	{ GTP_FWD_RELOC_COMPL, "Forward Relocation Complete" },
	{ GTP_RELOC_CANCEL_REQ,"Relocation Cancel Request" },
	{ GTP_RELOC_CANCEL_RSP,"Relocation Cancel Response" },
	{ GTP_FWD_SRNS,        "Forward SRNS Context" },
	{ GTP_FWD_RELOC_ACK,   "Forward Relocation Complete Acknowledge" },
	{ GTP_FWD_SRNS_ACK,    "Forward SRNS Context Acknowledge" },
	{ GTP_DATA_TRAN_REQ,   "Data Record Transfer Request" },
	{ GTP_DATA_TRAN_RSP,   "Data Record Transfer Response" },
	{ GTP_GPDU,            "G-PDU" },
	{ 0, NULL }
};

/* gtp_new */
/* gtp_free */

int gtp_newpdp(struct gsn_t *gsn, struct pdp_t **pdp,
	       uint64_t imsi, uint8_t nsapi)
{
	int rc;
	rc = pdp_newpdp(pdp, imsi, nsapi, NULL);
	if (!rc && *pdp)
		(*pdp)->gsn = gsn;
	return rc;
}

int gtp_freepdp(struct gsn_t *gsn, struct pdp_t *pdp)
{
	if (gsn->cb_delete_context)
		gsn->cb_delete_context(pdp);
	return pdp_freepdp(pdp);
}

/* Free pdp and all its secondary PDP contexts. Must be called on the primary PDP context. */
int gtp_freepdp_teardown(struct gsn_t *gsn, struct pdp_t *pdp)
{
	int n;
	struct pdp_t *secondary_pdp;
	OSMO_ASSERT(!pdp->secondary);

	for (n = 0; n < PDP_MAXNSAPI; n++) {
		if (pdp->secondary_tei[n]) {
			if (pdp_getgtp1
			    (&secondary_pdp,
			     pdp->secondary_tei[n])) {
				LOGP(DLGTP, LOGL_ERROR,
					"Unknown secondary PDP context\n");
				continue;
			}
			if (pdp != secondary_pdp) {
				gtp_freepdp(gsn, secondary_pdp);
			}
		}
	}

	return gtp_freepdp(gsn, pdp);
}

/* gtp_gpdu */

extern int gtp_fd(struct gsn_t *gsn)
{
	return gsn->fd0;
}

/* gtp_decaps */
/* gtp_retrans */
/* gtp_retranstimeout */

int gtp_set_cb_unsup_ind(struct gsn_t *gsn,
			 int (*cb) (struct sockaddr_in * peer))
{
	gsn->cb_unsup_ind = cb;
	return 0;
}

int gtp_set_cb_extheader_ind(struct gsn_t *gsn,
			     int (*cb) (struct sockaddr_in * peer))
{
	gsn->cb_extheader_ind = cb;
	return 0;
}

/* API: Initialise delete context callback */
/* Called whenever a pdp context is deleted for any reason */
int gtp_set_cb_delete_context(struct gsn_t *gsn, int (*cb) (struct pdp_t * pdp))
{
	gsn->cb_delete_context = cb;
	return 0;
}

int gtp_set_cb_conf(struct gsn_t *gsn,
		    int (*cb) (int type, int cause,
			       struct pdp_t * pdp, void *cbp))
{
	gsn->cb_conf = cb;
	return 0;
}

static void emit_cb_recovery(struct gsn_t *gsn, struct sockaddr_in * peer,
			     struct pdp_t * pdp, uint8_t recovery)
{
	if (gsn->cb_recovery)
		gsn->cb_recovery(peer, recovery);
	if (gsn->cb_recovery2)
		gsn->cb_recovery2(peer, pdp, recovery);
}

int gtp_set_cb_recovery(struct gsn_t *gsn,
			int (*cb) (struct sockaddr_in * peer, uint8_t recovery))
{
	gsn->cb_recovery = cb;
	return 0;
}

/* cb_recovery()
 * pdp may be NULL if Recovery IE was received from a message independent
 * of any PDP ctx (such as Echo Response), or because pdp ctx is unknown to the
 * local setup. In case pdp is known, caller may want to keep that pdp alive to
 * handle subsequent msg cb as this specific pdp ctx is still valid according to
 * specs.
 */
int gtp_set_cb_recovery2(struct gsn_t *gsn,
			int (*cb_recovery2) (struct sockaddr_in * peer, struct pdp_t * pdp, uint8_t recovery))
{
	gsn->cb_recovery2 = cb_recovery2;
	return 0;
}

int gtp_set_cb_data_ind(struct gsn_t *gsn,
			       int (*cb_data_ind) (struct pdp_t * pdp,
						   void *pack, unsigned len))
{
	gsn->cb_data_ind = cb_data_ind;
	return 0;
}

/**
 * get_default_gtp()
 * Generate a GPRS Tunneling Protocol signalling packet header, depending
 * on GTP version and message type. pdp is used for teid/flow label.
 * *packet must be allocated by the calling function, and be large enough
 * to hold the packet header.
 * returns the length of the header. 0 on error.
 **/
static unsigned int get_default_gtp(uint8_t version, uint8_t type, void *packet)
{
	struct gtp0_header *gtp0_default = (struct gtp0_header *)packet;
	struct gtp1_header_long *gtp1_default =
	    (struct gtp1_header_long *)packet;
	switch (version) {
	case 0:
		/* Initialise "standard" GTP0 header */
		memset(gtp0_default, 0, sizeof(struct gtp0_header));
		gtp0_default->flags = 0x1e;
		gtp0_default->type = hton8(type);
		gtp0_default->spare1 = 0xff;
		gtp0_default->spare2 = 0xff;
		gtp0_default->spare3 = 0xff;
		gtp0_default->number = 0xff;
		return GTP0_HEADER_SIZE;
	case 1:
		/* Initialise "standard" GTP1 header */
		/* 29.060: 8.2: S=1 and PN=0 */
		/* 29.060 9.3.1: For GTP-U messages Echo Request, Echo Response */
		/* and Supported Extension Headers Notification, the S field shall be */
		/* set to 1 */
		/* Currently extension headers are not supported */
		memset(gtp1_default, 0, sizeof(struct gtp1_header_long));
		/* No extension, enable sequence, no N-PDU */
		gtp1_default->flags = GTPHDR_F_VER(1) | GTP1HDR_F_GTP1 | GTP1HDR_F_SEQ;
		gtp1_default->type = hton8(type);
		return GTP1_HEADER_SIZE_LONG;
	default:
		LOGP(DLGTP, LOGL_ERROR,
			"Unknown GTP packet version: %d\n", version);
		return 0;
	}
}

/**
 * get_seq()
 * Get sequence number of a packet.
 * Returns 0 on error
 **/
static uint16_t get_seq(void *pack)
{
	union gtp_packet *packet = (union gtp_packet *)pack;
	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);

	if (ver == 0) {
		return ntoh16(packet->gtp0.h.seq);
	} else if (ver == 1 && (packet->flags & GTP1HDR_F_SEQ)) {	/* Version 1 with seq */
		return ntoh16(packet->gtp1l.h.seq);
	} else {
		return 0;
	}
}

/**
 * get_tid()
 * Get tunnel identifier of a packet.
 * Returns 0 on error
 **/
static uint64_t get_tid(void *pack)
{
	union gtp_packet *packet = (union gtp_packet *)pack;

	if (GTPHDR_F_GET_VER(packet->flags) == 0) {	/* Version 0 */
		return be64toh(packet->gtp0.h.tid);
	}
	return 0;
}

/**
 * get_hlen()
 * Get the header length of a packet.
 * Returns 0 on error
 **/
static uint16_t get_hlen(void *pack)
{
	union gtp_packet *packet = (union gtp_packet *)pack;
	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);

	if (ver == 0) {	/* Version 0 */
		return GTP0_HEADER_SIZE;
	} else if (ver == 1 && (packet->flags & 0x07) == 0) {	/* Short version 1 */
		return GTP1_HEADER_SIZE_SHORT;
	} else if (ver == 1) {	/* Version 1 with seq/n-pdu/ext */
		return GTP1_HEADER_SIZE_LONG;
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown packet flags: 0x%02x\n", packet->flags);
		return 0;
	}
}

/**
 * get_tei()
 * Get the tunnel endpoint identifier (flow label) of a packet.
 * Returns 0xffffffff on error.
 **/
static uint32_t get_tei(void *pack)
{
	union gtp_packet *packet = (union gtp_packet *)pack;
	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);

	if (ver == 0) {	/* Version 0 */
		return ntoh16(packet->gtp0.h.flow);
	} else if (ver == 1) {	/* Version 1 */
		return ntoh32(packet->gtp1l.h.tei);
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown packet flags: 0x%02x\n", packet->flags);
		return 0xffffffff;
	}
}

/* ***********************************************************
 * Reliable delivery of signalling messages
 *
 * Sequence numbers are used for both signalling messages and
 * data messages.
 *
 * For data messages each tunnel maintains a sequence counter,
 * which is incremented by one each time a new data message
 * is sent. The sequence number starts at (0) zero at tunnel
 * establishment, and wraps around at 65535 (29.060 9.3.1.1
 * and 09.60 8.1.1.1). The sequence numbers are either ignored,
 * or can be used to check the validity of the message in the
 * receiver, or for reordering af packets.
 *
 * For signalling messages the sequence number is used by
 * signalling messages for which a response is defined. A response
 * message should copy the sequence from the corresponding request
 * message. The sequence number "unambiguously" identifies a request
 * message within a given path, with a path being defined as a set of
 * two endpoints (29.060 8.2, 29.060 7.6, 09.60 7.8). "All request
 * messages shall be responded to, and all response messages associated
 * with a certain request shall always include the same information"
 *
 * We take this to mean that the GSN transmitting a request is free to
 * choose the sequence number, as long as it is unique within a given path.
 * It means that we are allowed to count backwards, or roll over at 17
 * if we prefer that. It also means that we can use the same counter for
 * all paths. This has the advantage that the transmitted request sequence
 * numbers are unique within each GSN, and also we dont have to mess around
 * with path setup and teardown.
 *
 * If a response message is lost, the request will be retransmitted, and
 * the receiving GSN will receive a "duplicated" request. The standard
 * requires the receiving GSN to send a response, with the same information
 * as in the original response. For most messages this happens automatically:
 *
 * Echo: Automatically duplicates the original response
 * Create pdp context: The SGSN may send create context request even if
 *   a context allready exist (imsi+nsapi?). This means that the reply will
     automatically duplicate the original response. It might however have
 *   side effects in the application which is asked twice to validate
 *   the login.
 * Update pdp context: Automatically duplicates the original response???
 * Delete pdp context. Automatically in gtp0, but in gtp1 will generate
 *   a nonexist reply message.
 *
 * The correct solution will be to make a queue containing response messages.
 * This queue should be checked whenever a request is received. If the
 * response is allready in the queue that response should be transmitted.
 * It should be possible to find messages in this queue on the basis of
 * the sequence number and peer GSN IP address (The sequense number is unique
 * within each path). This need to be implemented by a hash table. Furthermore
 * it should be possibly to delete messages based on a timeout. This can be
 * achieved by means of a linked list. The timeout value need to be larger
 * than T3-RESPONSE * N3-REQUESTS (recommended value 5). These timers are
 * set in the peer GSN, so there is no way to know these parameters. On the
 * other hand the timeout value need to be so small that we do not receive
 * wraparound sequence numbere before the message is deleted. 60 seconds is
 * probably not a bad choise.
 *
 * This queue however is first really needed from gtp1.
 *
 * gtp_req:
 *   Send off a signalling message with appropiate sequence
 *   number. Store packet in queue.
 * gtp_conf:
 *   Remove an incoming confirmation from the queue
 * gtp_resp:
 *   Send off a response to a request. Use the same sequence
 *   number in the response as in the request.
 * gtp_notification:
 *   Send off a notification message. This is neither a request nor
 *   a response. Both TEI and SEQ are zero.
 * gtp_retrans:
 *   Retransmit any outstanding packets which have exceeded
 *   a predefined timeout.
 *************************************************************/

static int gtp_req(struct gsn_t *gsn, uint8_t version, struct pdp_t *pdp,
	    union gtp_packet *packet, int len,
	    struct in_addr *inetaddr, void *cbp)
{
	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);
	struct sockaddr_in addr;
	struct qmsg_t *qmsg;
	int fd;

	memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr = *inetaddr;
#if defined(__FreeBSD__) || defined(__APPLE__)
	addr.sin_len = sizeof(addr);
#endif

	if (ver == 0) {	/* Version 0 */
		addr.sin_port = htons(GTP0_PORT);
		packet->gtp0.h.length = hton16(len - GTP0_HEADER_SIZE);
		packet->gtp0.h.seq = hton16(gsn->seq_next);
		if (pdp) {
			packet->gtp0.h.tid =
				htobe64(pdp_gettid(pdp->imsi, pdp->nsapi));
		}
		if (pdp && ((packet->gtp0.h.type == GTP_GPDU)
			    || (packet->gtp0.h.type == GTP_ERROR)))
			packet->gtp0.h.flow = hton16(pdp->flru);
		else if (pdp)
			packet->gtp0.h.flow = hton16(pdp->flrc);
		fd = gsn->fd0;
	} else if (ver == 1 && (packet->flags & GTP1HDR_F_SEQ)) {	/* Version 1 with seq */
		addr.sin_port = htons(GTP1C_PORT);
		packet->gtp1l.h.length = hton16(len - GTP1_HEADER_SIZE_SHORT);
		packet->gtp1l.h.seq = hton16(gsn->seq_next);
		if (pdp && ((packet->gtp1l.h.type == GTP_GPDU) ||
			    (packet->gtp1l.h.type == GTP_ERROR)))
			packet->gtp1l.h.tei = hton32(pdp->teid_gn);
		else if (pdp)
			packet->gtp1l.h.tei = hton32(pdp->teic_gn);
		fd = gsn->fd1c;
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown packet flags: 0x%02x\n", packet->flags);
		return -1;
	}

	if (sendto(fd, packet, len, 0,
		   (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		gsn->err_sendto++;
		LOGP(DLGTP, LOGL_ERROR, "Sendto(fd=%d, msg=%lx, len=%d, dst=%s) failed: Error = %s\n", fd,
		     (unsigned long)&packet, len, inet_ntoa(addr.sin_addr), strerror(errno));
		return -1;
	}

	/* Use new queue structure */
	if (queue_newmsg(gsn->queue_req, &qmsg, &addr, gsn->seq_next)) {
		gsn->err_queuefull++;
		LOGP(DLGTP, LOGL_ERROR,
			"Retransmit queue is full\n");
	} else {
		memcpy(&qmsg->p, packet, sizeof(union gtp_packet));
		qmsg->l = len;
		qmsg->timeout = time(NULL) + T3_REQUEST; /* When to timeout */
		qmsg->retrans = 0;	/* No retransmissions so far */
		qmsg->cbp = cbp;
		qmsg->type = ntoh8(packet->gtp0.h.type);
		qmsg->fd = fd;
	}
	gsn->seq_next++;	/* Count up this time */
	return 0;
}


/**
 * @brief clear the request and response queue. Useful for debugging to reset "some" state.
 * @param gsn The GGSN instance
 */
void gtp_clear_queues(struct gsn_t *gsn)
{
	struct qmsg_t *qmsg;

	while (!queue_getfirst(gsn->queue_req, &qmsg)) {
		queue_freemsg(gsn->queue_req, qmsg);
	}

	while (!queue_getfirst(gsn->queue_resp, &qmsg)) {
		queue_freemsg(gsn->queue_resp, qmsg);
	}
}

/* gtp_conf
 * Remove signalling packet from retransmission queue.
 * return 0 on success, EOF if packet was not found */

static int gtp_conf(struct gsn_t *gsn, uint8_t version, struct sockaddr_in *peer,
	     union gtp_packet *packet, int len, uint8_t * type, void **cbp)
{
	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);
	uint16_t seq;

	if (ver == 0)
		seq = ntoh16(packet->gtp0.h.seq);
	else if (ver == 1 && (packet->gtp1l.h.flags & GTP1HDR_F_SEQ))
		seq = ntoh16(packet->gtp1l.h.seq);
	else {
		GTP_LOGPKG(LOGL_ERROR, peer, packet, len,
			    "Unknown GTP packet version\n");
		return EOF;
	}

	if (queue_freemsg_seq(gsn->queue_req, peer, seq, type, cbp)) {
		gsn->err_seq++;
		GTP_LOGPKG(LOGL_ERROR, peer, packet, len,
			    "Confirmation packet not found in queue\n");
		return EOF;
	}

	return 0;
}

int gtp_retrans(struct gsn_t *gsn)
{
	/* Retransmit any outstanding packets */
	/* Remove from queue if maxretrans exceeded */
	time_t now;
	struct qmsg_t *qmsg;
	now = time(NULL);
	/*printf("Retrans: New beginning %d\n", (int) now); */

	/* get first element in queue, as long as the timeout of that
	 * element has expired */
	while ((!queue_getfirst(gsn->queue_req, &qmsg)) &&
	       (qmsg->timeout <= now)) {
		/*printf("Retrans timeout found: %d\n", (int) time(NULL)); */
		if (qmsg->retrans > N3_REQUESTS) {	/* To many retrans */
			if (gsn->cb_conf)
				gsn->cb_conf(qmsg->type, EOF, NULL, qmsg->cbp);
			queue_freemsg(gsn->queue_req, qmsg);
		} else {
			if (sendto(qmsg->fd, &qmsg->p, qmsg->l, 0,
				   (struct sockaddr *)&qmsg->peer,
				   sizeof(struct sockaddr_in)) < 0) {
				gsn->err_sendto++;
				LOGP(DLGTP, LOGL_ERROR,
					"Sendto(fd0=%d, msg=%lx, len=%d) failed: Error = %s\n",
					gsn->fd0, (unsigned long)&qmsg->p,
					qmsg->l, strerror(errno));
			}
			queue_back(gsn->queue_req, qmsg);
			qmsg->timeout = now + T3_REQUEST;
			qmsg->retrans++;
		}
	}

	/* Also clean up reply timeouts */
	while ((!queue_getfirst(gsn->queue_resp, &qmsg)) &&
	       (qmsg->timeout < now)) {
		/*printf("Retrans (reply) timeout found: %d\n", (int) time(NULL)); */
		queue_freemsg(gsn->queue_resp, qmsg);
	}

	return 0;
}

int gtp_retranstimeout(struct gsn_t *gsn, struct timeval *timeout)
{
	time_t now, later, diff;
	struct qmsg_t *qmsg;
	timeout->tv_usec = 0;

	if (queue_getfirst(gsn->queue_req, &qmsg)) {
		timeout->tv_sec = 10;
	} else {
		now = time(NULL);
		later = qmsg->timeout;
		timeout->tv_sec = later - now;
		if (timeout->tv_sec < 0)
			timeout->tv_sec = 0;	/* No negative allowed */
		if (timeout->tv_sec > 10)
			timeout->tv_sec = 10;	/* Max sleep for 10 sec */
	}

	if (queue_getfirst(gsn->queue_resp, &qmsg)) {
		/* already set by queue_req, do nothing */
	} else { /* trigger faster if earlier timeout exists in queue_resp */
		now = time(NULL);
		later = qmsg->timeout;
		diff = later - now;
		if (diff < 0)
			diff = 0;
		if (diff < timeout->tv_sec)
			timeout->tv_sec = diff;
	}

	return 0;
}

static int gtp_resp(uint8_t version, struct gsn_t *gsn, struct pdp_t *pdp,
	     union gtp_packet *packet, int len,
	     struct sockaddr_in *peer, int fd, uint16_t seq, uint64_t tid)
{
	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);
	struct qmsg_t *qmsg;

	if (ver == 0) {	/* Version 0 */
		packet->gtp0.h.length = hton16(len - GTP0_HEADER_SIZE);
		packet->gtp0.h.seq = hton16(seq);
		packet->gtp0.h.tid = htobe64(tid);
		if (pdp && ((packet->gtp0.h.type == GTP_GPDU) ||
			    (packet->gtp0.h.type == GTP_ERROR)))
			packet->gtp0.h.flow = hton16(pdp->flru);
		else if (pdp)
			packet->gtp0.h.flow = hton16(pdp->flrc);
	} else if (ver == 1 && (packet->flags & GTP1HDR_F_SEQ)) {	/* Version 1 with seq */
		packet->gtp1l.h.length = hton16(len - GTP1_HEADER_SIZE_SHORT);
		packet->gtp1l.h.seq = hton16(seq);
		if (pdp && (fd == gsn->fd1u))
			packet->gtp1l.h.tei = hton32(pdp->teid_gn);
		else if (pdp)
			packet->gtp1l.h.tei = hton32(pdp->teic_gn);
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown packet flags: 0x%02x\n", packet->flags);
		return -1;
	}

	if (fcntl(fd, F_SETFL, 0)) {
		LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
		return -1;
	}

	if (sendto(fd, packet, len, 0,
		   (struct sockaddr *)peer, sizeof(struct sockaddr_in)) < 0) {
		gsn->err_sendto++;
		LOGP(DLGTP, LOGL_ERROR,
			"Sendto(fd=%d, msg=%lx, len=%d) failed: Error = %s\n", fd,
			(unsigned long)&packet, len, strerror(errno));
		return -1;
	}

	/* Use new queue structure */
	if (queue_newmsg(gsn->queue_resp, &qmsg, peer, seq)) {
		gsn->err_queuefull++;
		LOGP(DLGTP, LOGL_ERROR, "Retransmit queue is full\n");
	} else {
		memcpy(&qmsg->p, packet, sizeof(union gtp_packet));
		qmsg->l = len;
		qmsg->timeout = time(NULL) + 60;	/* When to timeout */
		qmsg->retrans = 0;	/* No retransmissions so far */
		qmsg->cbp = NULL;
		qmsg->type = 0;
		qmsg->fd = fd;
	}
	return 0;
}

static int gtp_notification(struct gsn_t *gsn, uint8_t version,
		     union gtp_packet *packet, int len,
		     struct sockaddr_in *peer, int fd, uint16_t seq)
{

	uint8_t ver = GTPHDR_F_GET_VER(packet->flags);
	struct sockaddr_in addr;

	memcpy(&addr, peer, sizeof(addr));

	/* In GTP0 notifications are treated as replies. In GTP1 they
	   are requests for which there is no reply */

	if (fd == gsn->fd1c)
		addr.sin_port = htons(GTP1C_PORT);
	else if (fd == gsn->fd1u)
		addr.sin_port = htons(GTP1C_PORT);

	if (ver == 0) {	/* Version 0 */
		packet->gtp0.h.length = hton16(len - GTP0_HEADER_SIZE);
		packet->gtp0.h.seq = hton16(seq);
	} else if (ver == 1 && (packet->flags & GTP1HDR_F_SEQ)) {	/* Version 1 with seq */
		packet->gtp1l.h.length = hton16(len - GTP1_HEADER_SIZE_SHORT);
		packet->gtp1l.h.seq = hton16(seq);
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown packet flags: 0x%02x\n", packet->flags);
		return -1;
	}

	if (fcntl(fd, F_SETFL, 0)) {
		LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
		return -1;
	}

	if (sendto(fd, packet, len, 0,
		   (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0) {
		gsn->err_sendto++;
		LOGP(DLGTP, LOGL_ERROR,
			"Sendto(fd=%d, msg=%lx, len=%d) failed: Error = %s\n", fd,
			(unsigned long)&packet, len, strerror(errno));
		return -1;
	}
	return 0;
}

static int gtp_duplicate(struct gsn_t *gsn, uint8_t version,
		  struct sockaddr_in *peer, uint16_t seq)
{
	struct qmsg_t *qmsg;

	if (queue_seqget(gsn->queue_resp, &qmsg, peer, seq)) {
		return EOF;	/* Notfound */
	}

	if (fcntl(qmsg->fd, F_SETFL, 0)) {
		LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
		return -1;
	}

	if (sendto(qmsg->fd, &qmsg->p, qmsg->l, 0,
		   (struct sockaddr *)peer, sizeof(struct sockaddr_in)) < 0) {
		gsn->err_sendto++;
		LOGP(DLGTP, LOGL_ERROR,
			"Sendto(fd=%d, msg=%lx, len=%d) failed: Error = %s\n",
			qmsg->fd, (unsigned long)&qmsg->p, qmsg->l,
			strerror(errno));
	}
	return 0;
}

/* Perform restoration and recovery error handling as described in 29.060 */
static void log_restart(struct gsn_t *gsn)
{
	FILE *f;
	int i, rc;
	int counter = 0;
	char *filename;

	filename = talloc_asprintf(NULL, "%s/%s", gsn->statedir, RESTART_FILE);
	OSMO_ASSERT(filename);

	/* We try to open file. On failure we will later try to create file */
	if (!(f = fopen(filename, "r"))) {
		LOGP(DLGTP, LOGL_NOTICE,
			"State information file (%s) not found. Creating new file.\n",
			filename);
	} else {
		rc = fscanf(f, "%d", &counter);
		if (rc != 1) {
			LOGP(DLGTP, LOGL_ERROR,
				"fscanf failed to read counter value\n");
			goto close_file;
		}
		if (fclose(f)) {
			LOGP(DLGTP, LOGL_ERROR,
				"fclose failed: Error = %s\n", strerror(errno));
		}
	}

	gsn->restart_counter = (unsigned char)counter;
	gsn->restart_counter++;

	/* Keep the umask closely wrapped around our fopen() call in case the
	 * log outputs cause file creation. */
	i = umask(022);
	f = fopen(filename, "w");
	umask(i);
	if (!f) {
		LOGP(DLGTP, LOGL_ERROR,
			"fopen(path=%s, mode=%s) failed: Error = %s\n", filename,
			"w", strerror(errno));
		goto free_filename;
	}

	fprintf(f, "%d\n", gsn->restart_counter);
close_file:
	if (fclose(f))
		LOGP(DLGTP, LOGL_ERROR,
			"fclose failed: Error = %s\n", strerror(errno));
free_filename:
	talloc_free(filename);
}

int gtp_new(struct gsn_t **gsn, char *statedir, struct in_addr *listen,
	    int mode)
{
	struct sockaddr_in addr;

	LOGP(DLGTP, LOGL_NOTICE, "GTP: gtp_newgsn() started at %s\n", inet_ntoa(*listen));

	*gsn = calloc(sizeof(struct gsn_t), 1);	/* TODO */

	(*gsn)->statedir = statedir;
	log_restart(*gsn);

	/* Initialise sequence number */
	(*gsn)->seq_next = (*gsn)->restart_counter * 1024;

	/* Initialise request retransmit queue */
	queue_new(&(*gsn)->queue_req);
	queue_new(&(*gsn)->queue_resp);

	/* Initialise pdp table */
	pdp_init();

	/* Initialise call back functions */
	(*gsn)->cb_create_context_ind = 0;
	(*gsn)->cb_delete_context = 0;
	(*gsn)->cb_unsup_ind = 0;
	(*gsn)->cb_conf = 0;
	(*gsn)->cb_data_ind = 0;

	/* Store function parameters */
	(*gsn)->gsnc = *listen;
	(*gsn)->gsnu = *listen;
	(*gsn)->mode = mode;

	/* Create GTP version 0 socket */
	if (((*gsn)->fd0 = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		(*gsn)->err_socket++;
		LOGP(DLGTP, LOGL_ERROR,
		     "GTPv0 socket(domain=%d, type=%d, protocol=%d) failed: Error = %s\n",
			AF_INET, SOCK_DGRAM, 0, strerror(errno));
		return -errno;
	}

	memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr = *listen;	/* Same IP for user traffic and signalling */
	addr.sin_port = htons(GTP0_PORT);
#if defined(__FreeBSD__) || defined(__APPLE__)
	addr.sin_len = sizeof(addr);
#endif

	if (bind((*gsn)->fd0, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		(*gsn)->err_socket++;
		LOGP_WITH_ADDR(DLGTP, LOGL_ERROR, addr,
			       "bind(fd0=%d) failed: Error = %s\n",
			       (*gsn)->fd0, strerror(errno));
		return -errno;
	}

	/* Create GTP version 1 control plane socket */
	if (((*gsn)->fd1c = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		(*gsn)->err_socket++;
		LOGP(DLGTP, LOGL_ERROR,
		     "GTPv1 control plane socket(domain=%d, type=%d, protocol=%d) failed: Error = %s\n",
			AF_INET, SOCK_DGRAM, 0, strerror(errno));
		return -errno;
	}

	memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr = *listen;	/* Same IP for user traffic and signalling */
	addr.sin_port = htons(GTP1C_PORT);
#if defined(__FreeBSD__) || defined(__APPLE__)
	addr.sin_len = sizeof(addr);
#endif

	if (bind((*gsn)->fd1c, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		(*gsn)->err_socket++;
		LOGP_WITH_ADDR(DLGTP, LOGL_ERROR, addr,
				"bind(fd1c=%d) failed: Error = %s\n",
				(*gsn)->fd1c, strerror(errno));
		return -errno;
	}

	/* Create GTP version 1 user plane socket */
	if (((*gsn)->fd1u = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		(*gsn)->err_socket++;
		LOGP(DLGTP, LOGL_ERROR,
		     "GTPv1 user plane socket(domain=%d, type=%d, protocol=%d) failed: Error = %s\n",
			AF_INET, SOCK_DGRAM, 0, strerror(errno));
		return -errno;
	}

	memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr = *listen;	/* Same IP for user traffic and signalling */
	addr.sin_port = htons(GTP1U_PORT);
#if defined(__FreeBSD__) || defined(__APPLE__)
	addr.sin_len = sizeof(addr);
#endif

	if (bind((*gsn)->fd1u, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		(*gsn)->err_socket++;
		LOGP_WITH_ADDR(DLGTP, LOGL_ERROR, addr,
			"bind(fd1u=%d) failed: Error = %s\n",
			(*gsn)->fd1u, strerror(errno));
		return -errno;
	}

	return 0;
}

int gtp_free(struct gsn_t *gsn)
{

	/* Clean up retransmit queues */
	queue_free(gsn->queue_req);
	queue_free(gsn->queue_resp);

	close(gsn->fd0);
	close(gsn->fd1c);
	close(gsn->fd1u);

	free(gsn);
	return 0;
}

/* ***********************************************************
 * Path management messages
 * Messages: echo and version not supported.
 * A path is connection between two UDP/IP endpoints
 *
 * A path is either using GTP0 or GTP1. A path can be
 * established by any kind of GTP message??

 * Which source port to use?
 * GTP-C request destination port is 2123/3386
 * GTP-U request destination port is 2152/3386
 * T-PDU destination port is 2152/3386.
 * For the above messages the source port is locally allocated.
 * For response messages src=rx-dst and dst=rx-src.
 * For simplicity we should probably use 2123+2152/3386 as
 * src port even for the cases where src can be locally
 * allocated. This also means that we have to listen only to
 * the same ports.
 * For response messages we need to be able to respond to
 * the relevant src port even if it is locally allocated by
 * the peer.
 *
 * The need for path management!
 * We might need to keep a list of active paths. This might
 * be in the form of remote IP address + UDP port numbers.
 * (We will consider a path astablished if we have a context
 * with the node in question)
 *************************************************************/

/* Send off an echo request */
int gtp_echo_req(struct gsn_t *gsn, int version, void *cbp,
		 struct in_addr *inetaddr)
{
	union gtp_packet packet;
	unsigned int length = get_default_gtp(version, GTP_ECHO_REQ, &packet);
	return gtp_req(gsn, version, NULL, &packet, length, inetaddr, cbp);
}

/* Send off an echo reply */
int gtp_echo_resp(struct gsn_t *gsn, int version,
		  struct sockaddr_in *peer, int fd, void *pack, unsigned len)
{
	union gtp_packet packet;
	unsigned int length = get_default_gtp(version, GTP_ECHO_RSP, &packet);
	gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_RECOVERY,
		  gsn->restart_counter);
	return gtp_resp(version, gsn, NULL, &packet, length, peer, fd,
			get_seq(pack), get_tid(pack));
}

/* Handle a received echo request */
int gtp_echo_ind(struct gsn_t *gsn, int version, struct sockaddr_in *peer,
		 int fd, void *pack, unsigned len)
{

	/* Check if it was a duplicate request */
	if (!gtp_duplicate(gsn, 0, peer, get_seq(pack)))
		return 0;

	/* Send off reply to request */
	return gtp_echo_resp(gsn, version, peer, fd, pack, len);
}

/* Handle a received echo reply */
int gtp_echo_conf(struct gsn_t *gsn, int version, struct sockaddr_in *peer,
		  void *pack, unsigned len)
{
	union gtpie_member *ie[GTPIE_SIZE];
	unsigned char recovery;
	void *cbp = NULL;
	uint8_t type = 0;
	int hlen = get_hlen(pack);

	/* Remove packet from queue */
	if (gtp_conf(gsn, version, peer, pack, len, &type, &cbp))
		return EOF;

	/* Extract information elements into a pointer array */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, NULL, cbp);
		return EOF;
	}

	if (gtpie_gettv1(ie, GTPIE_RECOVERY, 0, &recovery)) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory field\n");
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, NULL, cbp);
		return EOF;
	}

	/* Echo reply packages does not have a cause information element */
	/* Instead we return the recovery number in the callback function */
	if (gsn->cb_conf)
		gsn->cb_conf(type, recovery, NULL, cbp);

	emit_cb_recovery(gsn, peer, NULL, recovery);

	return 0;
}

/* Send off a Version Not Supported message */
/* This message is somewhat special in that it actually is a
 * response to some other message with unsupported GTP version
 * For this reason it has parameters like a response, and does
 * its own message transmission. No signalling queue is used
 * The reply is sent to the peer IP and peer UDP. This means that
 * the peer will be receiving a GTP0 message on a GTP1 port!
 * In practice however this will never happen as a GTP0 GSN will
 * only listen to the GTP0 port, and therefore will never receive
 * anything else than GTP0 */

int gtp_unsup_req(struct gsn_t *gsn, int version, struct sockaddr_in *peer,
		  int fd, void *pack, unsigned len)
{
	union gtp_packet packet;

	/* GTP 1 is the highest supported protocol */
	unsigned int length = get_default_gtp(1, GTP_NOT_SUPPORTED, &packet);
	return gtp_notification(gsn, version, &packet, length, peer, fd, 0);
}

/* Handle a Version Not Supported message */
int gtp_unsup_ind(struct gsn_t *gsn, struct sockaddr_in *peer,
		  void *pack, unsigned len)
{

	if (gsn->cb_unsup_ind)
		gsn->cb_unsup_ind(peer);

	return 0;
}

/* Send off an Supported Extension Headers Notification */
static int gtp_extheader_req(struct gsn_t *gsn, uint8_t version, struct sockaddr_in *peer,
		      int fd, void *pack, unsigned len)
{
	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(version, GTP_SUPP_EXT_HEADER, &packet);

	uint8_t pdcp_pdu = GTP_EXT_PDCP_PDU;

	if (version < 1)
		return 0;

	/* We report back that we support only PDCP PDU headers */
	gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EXT_HEADER_T,
		  sizeof(pdcp_pdu), &pdcp_pdu);

	return gtp_notification(gsn, version, &packet, length,
				peer, fd, get_seq(pack));
}

/* Handle a Supported Extension Headers Notification */
static int gtp_extheader_ind(struct gsn_t *gsn, struct sockaddr_in *peer,
		      void *pack, unsigned len)
{

	if (gsn->cb_extheader_ind)
		gsn->cb_extheader_ind(peer);

	return 0;
}

/* ***********************************************************
 * Session management messages
 * Messages: create, update and delete PDP context
 *
 * Information storage
 * Information storage for each PDP context is defined in
 * 23.060 section 13.3. Includes IMSI, MSISDN, APN, PDP-type,
 * PDP-address (IP address), sequence numbers, charging ID.
 * For the SGSN it also includes radio related mobility
 * information.
 *************************************************************/

/* API: Send Create PDP Context Request (7.3.1) */
int gtp_create_context_req(struct gsn_t *gsn, struct pdp_t *pdp,
				  void *cbp)
{
	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(pdp->version, GTP_CREATE_PDP_REQ, &packet);
	struct pdp_t *linked_pdp = NULL;

	/* TODO: Secondary PDP Context Activation Procedure */
	/* In secondary activation procedure the PDP context is identified
	   by tei in the header. The following fields are omitted: Selection
	   mode, IMSI, MSISDN, End User Address, Access Point Name and
	   Protocol Configuration Options */

	if (pdp->secondary) {
		if (pdp_getgtp1(&linked_pdp, pdp->teic_own)) {
			LOGP(DLGTP, LOGL_ERROR,
				"Unknown linked PDP context: %u\n", pdp->teic_own);
			return EOF;
		}
	}

	if (pdp->version == 0) {
		gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE0,
			  sizeof(pdp->qos_req0), pdp->qos_req0);
	}

	/* Section 7.7.2 */
	if (pdp->version == 1) {
		if (!pdp->secondary)	/* Not Secondary PDP Context Activation Procedure */
			gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_IMSI,
				  sizeof(pdp->imsi), (uint8_t *) & pdp->imsi);
	}

	/* Section 7.7.3 Routing Area Information */
	if (pdp->rai_given == 1)
		gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_RAI,
			  pdp->rai.l, (uint8_t *) & pdp->rai.v);

	/* Section 7.7.11 */
	if (pdp->norecovery_given == 0)
		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_RECOVERY,
			  gsn->restart_counter);

	/* Section 7.7.12 */
	if (!pdp->secondary)	/* Not Secondary PDP Context Activation Procedure */
		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_SELECTION_MODE,
			  pdp->selmode);

	if (pdp->version == 0) {
		gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_DI, pdp->fllu);
		gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_C, pdp->fllc);
	}

	/* Section 7.7.13 */
	if (pdp->version == 1) {
		gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_DI,
			  pdp->teid_own);

		/* Section 7.7.14 */
		if (!pdp->teic_confirmed)
			gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_C,
				  pdp->teic_own);

		/* Section 7.7.17 */
		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_NSAPI, pdp->nsapi);

		/* Section 7.7.17 */
		if (pdp->secondary)	/* Secondary PDP Context Activation Procedure */
			gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_NSAPI,
				  linked_pdp->nsapi);

		/* Section 7.7.23 */
		if (pdp->cch_pdp)	/* Only include charging if flags are set */
			gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_CHARGING_C,
				  pdp->cch_pdp);
	}

	/* TODO
	   gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_TRACE_REF,
	   pdp->traceref);
	   gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_TRACE_TYPE,
	   pdp->tracetype); */

	/* Section 7.7.27 */
	if (!pdp->secondary)	/* Not Secondary PDP Context Activation Procedure */
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA,
			  pdp->eua.l, pdp->eua.v);

	/* Section 7.7.30 */
	if (!pdp->secondary)	/* Not Secondary PDP Context Activation Procedure */
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_APN,
			  pdp->apn_use.l, pdp->apn_use.v);

	/* Section 7.7.31 */
	if (!pdp->secondary)	/* Not Secondary PDP Context Activation Procedure */
		if (pdp->pco_req.l)
			gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_PCO,
				  pdp->pco_req.l, pdp->pco_req.v);

	/* Section 7.7.32 */
	gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
		  pdp->gsnlc.l, pdp->gsnlc.v);
	/* Section 7.7.32 */
	gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
		  pdp->gsnlu.l, pdp->gsnlu.v);

	/* Section 7.7.33 */
	if (!pdp->secondary)	/* Not Secondary PDP Context Activation Procedure */
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_MSISDN,
			  pdp->msisdn.l, pdp->msisdn.v);

	/* Section 7.7.34 */
	if (pdp->version == 1)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE,
			  pdp->qos_req.l, pdp->qos_req.v);

	/* Section 7.7.36 */
	if ((pdp->version == 1) && pdp->tft.l)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_TFT,
			  pdp->tft.l, pdp->tft.v);

	/* Section 7.7.41 */
	if ((pdp->version == 1) && pdp->triggerid.l)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_TRIGGER_ID,
			  pdp->triggerid.l, pdp->triggerid.v);

	/* Section 7.7.42 */
	if ((pdp->version == 1) && pdp->omcid.l)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_OMC_ID,
			  pdp->omcid.l, pdp->omcid.v);

	/* new R7 fields */
	if (pdp->rattype_given == 1)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_RAT_TYPE,
			  pdp->rattype.l, pdp->rattype.v);

	if (pdp->userloc_given == 1)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_USER_LOC,
			  pdp->userloc.l, pdp->userloc.v);

	if (pdp->mstz_given == 1)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_MS_TZ,
			  pdp->mstz.l, pdp->mstz.v);

	if (pdp->imeisv_given == 1)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_IMEI_SV,
			  pdp->imeisv.l, pdp->imeisv.v);

	/* TODO hisaddr0 */
	gtp_req(gsn, pdp->version, pdp, &packet, length, &pdp->hisaddr0, cbp);

	return 0;
}

/* API: Application response to context indication */
int gtp_create_context_resp(struct gsn_t *gsn, struct pdp_t *pdp, int cause)
{

	/* Now send off a reply to the peer */
	gtp_create_pdp_resp(gsn, pdp->version, pdp, cause);

	if (cause != GTPCAUSE_ACC_REQ) {
		pdp_freepdp(pdp);
	}

	return 0;
}

/* API: Register create context indication callback */
int gtp_set_cb_create_context_ind(struct gsn_t *gsn,
				  int (*cb_create_context_ind) (struct pdp_t *
								pdp))
{
	gsn->cb_create_context_ind = cb_create_context_ind;
	return 0;
}

/* Send Create PDP Context Response */
int gtp_create_pdp_resp(struct gsn_t *gsn, int version, struct pdp_t *pdp,
			uint8_t cause)
{
	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(version, GTP_CREATE_PDP_RSP, &packet);

	gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_CAUSE, cause);

	if (cause == GTPCAUSE_ACC_REQ) {

		if (version == 0)
			gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE0,
				  sizeof(pdp->qos_neg0), pdp->qos_neg0);

		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_REORDER,
			  pdp->reorder);
		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_RECOVERY,
			  gsn->restart_counter);

		if (version == 0) {
			gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_DI,
				  pdp->fllu);
			gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_C,
				  pdp->fllc);
		}

		if (version == 1) {
			gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_DI,
				  pdp->teid_own);
			gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_C,
				  pdp->teic_own);
		}

		/* TODO: We use teic_own as charging ID */
		gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_CHARGING_ID,
			  pdp->teic_own);

		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA,
			  pdp->eua.l, pdp->eua.v);

		if (pdp->pco_neg.l) {	/* Optional PCO */
			gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_PCO,
				  pdp->pco_neg.l, pdp->pco_neg.v);
		}

		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
			  pdp->gsnlc.l, pdp->gsnlc.v);
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
			  pdp->gsnlu.l, pdp->gsnlu.v);

		if (version == 1)
			gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE,
				  pdp->qos_neg.l, pdp->qos_neg.v);

		/* TODO: Charging gateway address */
	}

	return gtp_resp(version, gsn, pdp, &packet, length, &pdp->sa_peer,
			pdp->fd, pdp->seq, pdp->tid);
}

/* Handle Create PDP Context Request */
int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
		       struct sockaddr_in *peer, int fd,
		       void *pack, unsigned len)
{
	struct pdp_t *pdp, *pdp_old;
	struct pdp_t pdp_buf;
	union gtpie_member *ie[GTPIE_SIZE];
	uint8_t recovery;
	bool recovery_recvd = false;
	int rc;

	uint16_t seq = get_seq(pack);
	int hlen = get_hlen(pack);
	uint8_t linked_nsapi = 0;
	struct pdp_t *linked_pdp = NULL;

	if (!gtp_duplicate(gsn, version, peer, seq))
		return 0;

	pdp = &pdp_buf;
	memset(pdp, 0, sizeof(struct pdp_t));

	if (version == 0) {
		uint64_t tid = be64toh(((union gtp_packet *)pack)->gtp0.h.tid);

		pdp_set_imsi_nsapi(pdp, tid);
	}

	pdp->seq = seq;
	pdp->sa_peer = *peer;
	pdp->fd = fd;
	pdp->version = version;

	/* Decode information elements */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		if (0 == version)
			return EOF;
		else
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_INVALID_MESSAGE);
	}

	if (version == 1) {
		/* Linked NSAPI (conditional) */
		/* If included this is the Secondary PDP Context Activation Procedure */
		/* In secondary activation IMSI is not included, so the context must be */
		/* identified by the tei */
		if (!gtpie_gettv1(ie, GTPIE_NSAPI, 1, &linked_nsapi)) {

			/* Find the primary PDP context */
			if (pdp_getgtp1(&linked_pdp, get_tei(pack))) {
				gsn->incorrect++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Incorrect optional information field\n");
				return gtp_create_pdp_resp(gsn, version, pdp,
							   GTPCAUSE_OPT_IE_INCORRECT);
			}

			/* Check that the primary PDP context matches linked nsapi */
			if (linked_pdp->nsapi != linked_nsapi) {
				gsn->incorrect++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Incorrect optional information field\n");
				return gtp_create_pdp_resp(gsn, version, pdp,
							   GTPCAUSE_OPT_IE_INCORRECT);
			}

			/* Copy parameters from primary context */
			pdp->selmode = linked_pdp->selmode;
			pdp->imsi = linked_pdp->imsi;
			pdp->msisdn = linked_pdp->msisdn;
			pdp->eua = linked_pdp->eua;
			pdp->pco_req = linked_pdp->pco_req;
			pdp->apn_req = linked_pdp->apn_req;
			pdp->teic_gn = linked_pdp->teic_gn;
			pdp->secondary = 1;
		} else {
			/* Not Secondary PDP Context Activation Procedure */
			/* IMSI (conditional): If the MS is emergency attached
			   and the MS is UICCless, the IMSI cannot be included
			   in the message and therefore IMSI shall not be
			   included in the message. */
			if (gtpie_gettv0
			    (ie, GTPIE_IMSI, 0, &pdp->imsi, sizeof(pdp->imsi))) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer, pack,
					    len, "Missing IMSI not supported\n");
				return gtp_create_pdp_resp(gsn, version, pdp,
							   GTPCAUSE_MAN_IE_MISSING);
			}
		}

		/* TEID (mandatory) */
		if (gtpie_gettv4(ie, GTPIE_TEI_DI, 0, &pdp->teid_gn)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		/* TEIC (conditional) */
		if (!linked_pdp) {	/* Not Secondary PDP Context Activation Procedure */
			if (gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn)) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing mandatory information field\n");
				return gtp_create_pdp_resp(gsn, version, pdp,
							   GTPCAUSE_MAN_IE_MISSING);
			}
		}
		/* NSAPI (mandatory) */
		if (gtpie_gettv1(ie, GTPIE_NSAPI, 0, &pdp->nsapi)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		/* QoS (mandatory) */
		if (gtpie_gettlv(ie, GTPIE_QOS_PROFILE, 0, &pdp->qos_req.l,
				 &pdp->qos_req.v, sizeof(pdp->qos_req.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		/* TFT (conditional) */
		if (gtpie_gettlv(ie, GTPIE_TFT, 0, &pdp->tft.l,
				 &pdp->tft.v, sizeof(pdp->tft.v))) {
		}
	}
	/* if (version == 1) */
	if (version == 0) {
		if (gtpie_gettv0(ie, GTPIE_QOS_PROFILE0, 0,
				 pdp->qos_req0, sizeof(pdp->qos_req0))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		if (gtpie_gettv2(ie, GTPIE_FL_DI, 0, &pdp->flru)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		if (gtpie_gettv2(ie, GTPIE_FL_C, 0, &pdp->flrc)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
	}

	/* SGSN address for signalling (mandatory) */
	if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 0, &pdp->gsnrc.l,
			 &pdp->gsnrc.v, sizeof(pdp->gsnrc.v))) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory information field\n");
		return gtp_create_pdp_resp(gsn, version, pdp,
					   GTPCAUSE_MAN_IE_MISSING);
	}

	/* SGSN address for user traffic (mandatory) */
	if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 1, &pdp->gsnru.l,
			 &pdp->gsnru.v, sizeof(pdp->gsnru.v))) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory information field\n");
		return gtp_create_pdp_resp(gsn, version, pdp,
					   GTPCAUSE_MAN_IE_MISSING);
	}
	/* Recovery (optional) */
	if (!gtpie_gettv1(ie, GTPIE_RECOVERY, 0, &recovery)) {
		/* we use recovery futher down after announcing new pdp ctx to user */
		recovery_recvd = true;
	}

	if (!linked_pdp) {	/* Not Secondary PDP Context Activation Procedure */
		/* Selection mode (conditional) */
		if (gtpie_gettv0(ie, GTPIE_SELECTION_MODE, 0,
				 &pdp->selmode, sizeof(pdp->selmode))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		/* End User Address (conditional) */
		if (gtpie_gettlv(ie, GTPIE_EUA, 0, &pdp->eua.l,
				 &pdp->eua.v, sizeof(pdp->eua.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		/* APN */
		if (gtpie_gettlv(ie, GTPIE_APN, 0, &pdp->apn_req.l,
				 &pdp->apn_req.v, sizeof(pdp->apn_req.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
		/* Extract protocol configuration options (optional) */
		if (!gtpie_gettlv(ie, GTPIE_PCO, 0, &pdp->pco_req.l,
				  &pdp->pco_req.v, sizeof(pdp->pco_req.v))) {
		}
		/* MSISDN (conditional) */
		if (gtpie_gettlv(ie, GTPIE_MSISDN, 0, &pdp->msisdn.l,
				 &pdp->msisdn.v, sizeof(pdp->msisdn.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
	}

	/* Initialize our own IP addresses */
	in_addr2gsna(&pdp->gsnlc, &gsn->gsnc);
	in_addr2gsna(&pdp->gsnlu, &gsn->gsnu);

	DEBUGP(DLGTP, "gtp_create_pdp_ind: Before pdp_tidget\n");

	if (!pdp_getimsi(&pdp_old, pdp->imsi, pdp->nsapi)) {
		/* Found old pdp with same tid. Now the voodoo begins! */
		/* 09.60 / 29.060 allows create on existing context to "steal" */
		/* the context which was allready established */
		/* We check that the APN, selection mode and MSISDN is the same */
		DEBUGP(DLGTP, "gtp_create_pdp_ind: Old context found\n");
		if ((pdp->apn_req.l == pdp_old->apn_req.l)
		    &&
		    (!memcmp
		     (pdp->apn_req.v, pdp_old->apn_req.v, pdp->apn_req.l))
		    && (pdp->selmode == pdp_old->selmode)
		    && (pdp->msisdn.l == pdp_old->msisdn.l)
		    &&
		    (!memcmp(pdp->msisdn.v, pdp_old->msisdn.v, pdp->msisdn.l)))
		{
			/* OK! We are dealing with the same APN. We will copy new
			 * parameters to the old pdp and send off confirmation
			 * We ignore the following information elements:
			 * QoS: MS will get originally negotiated QoS.
			 * End user address (EUA). MS will get old EUA anyway.
			 * Protocol configuration option (PCO): Only application can verify */
			DEBUGP(DLGTP, "gtp_create_pdp_ind: Old context found\n");

			/* Copy remote flow label */
			pdp_old->flru = pdp->flru;
			pdp_old->flrc = pdp->flrc;

			/* Copy remote tei */
			pdp_old->teid_gn = pdp->teid_gn;
			pdp_old->teic_gn = pdp->teic_gn;

			/* Copy peer GSN address */
			pdp_old->gsnrc.l = pdp->gsnrc.l;
			memcpy(&pdp_old->gsnrc.v, &pdp->gsnrc.v, pdp->gsnrc.l);
			pdp_old->gsnru.l = pdp->gsnru.l;
			memcpy(&pdp_old->gsnru.v, &pdp->gsnru.v, pdp->gsnru.l);

			/* Copy request parameters */
			pdp_old->seq = pdp->seq;
			pdp_old->sa_peer = pdp->sa_peer;
			pdp_old->fd = pdp->fd = fd;
			pdp_old->version = pdp->version = version;

			/* Switch to using the old pdp context */
			pdp = pdp_old;

			if (recovery_recvd)
				emit_cb_recovery(gsn, peer, pdp, recovery);

			/* Confirm to peer that things were "successful" */
			return gtp_create_pdp_resp(gsn, version, pdp,
						   GTPCAUSE_ACC_REQ);
		} else {	/* This is not the same PDP context. Delete the old one. */

			DEBUGP(DLGTP, "gtp_create_pdp_ind: Deleting old context\n");

			gtp_freepdp(gsn, pdp_old);

			DEBUGP(DLGTP, "gtp_create_pdp_ind: Deleted...\n");
		}
	}

	pdp_newpdp(&pdp, pdp->imsi, pdp->nsapi, pdp);
	if (pdp)
		pdp->gsn = gsn;

	/* Callback function to validate login */
	if (gsn->cb_create_context_ind != 0)
		rc = gsn->cb_create_context_ind(pdp);
	else {
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "No create_context_ind callback defined\n");
		rc = gtp_create_pdp_resp(gsn, version, pdp,
					   GTPCAUSE_NOT_SUPPORTED);
	}
	if (recovery_recvd)
		emit_cb_recovery(gsn, peer, pdp, recovery);
	return rc;
}

/* Handle Create PDP Context Response */
int gtp_create_pdp_conf(struct gsn_t *gsn, int version,
			struct sockaddr_in *peer, void *pack, unsigned len)
{
	struct pdp_t *pdp;
	union gtpie_member *ie[GTPIE_SIZE];
	uint8_t cause, recovery;
	void *cbp = NULL;
	uint8_t type = 0;
	int hlen = get_hlen(pack);

	/* Remove packet from queue */
	if (gtp_conf(gsn, version, peer, pack, len, &type, &cbp))
		return EOF;

	/* Find the context in question */
	if (pdp_getgtp1(&pdp, get_tei(pack))) {
		gsn->err_unknownpdp++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Unknown PDP context: %u\n", get_tei(pack));
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, NULL, cbp);
		return EOF;
	}

	/* Register that we have received a valid teic from GGSN */
	pdp->teic_confirmed = 1;

	/* Decode information elements */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, pdp, cbp);
		/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
		   pdp_freepdp(pdp); */
		return EOF;
	}

	/* Extract cause value (mandatory) */
	if (gtpie_gettv1(ie, GTPIE_CAUSE, 0, &cause)) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory information field\n");
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, pdp, cbp);
		/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
		   pdp_freepdp(pdp); */
		return EOF;
	}

	/* Extract recovery (optional) */
	if (!gtpie_gettv1(ie, GTPIE_RECOVERY, 0, &recovery)) {
		emit_cb_recovery(gsn, peer, pdp, recovery);
	}

	/* Extract protocol configuration options (optional) */
	if (!gtpie_gettlv(ie, GTPIE_PCO, 0, &pdp->pco_req.l,
			  &pdp->pco_req.v, sizeof(pdp->pco_req.v))) {
	}

	/* Check all conditional information elements */
	if (GTPCAUSE_ACC_REQ == cause) {

		if (version == 0) {
			if (gtpie_gettv0(ie, GTPIE_QOS_PROFILE0, 0,
					 &pdp->qos_neg0,
					 sizeof(pdp->qos_neg0))) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing conditional information field\n");
				if (gsn->cb_conf)
					gsn->cb_conf(type, EOF, pdp, cbp);
				/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
				   pdp_freepdp(pdp); */
				return EOF;
			}
		}

		if (gtpie_gettv1(ie, GTPIE_REORDER, 0, &pdp->reorder)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len,
				    "Missing conditional information field\n");
			if (gsn->cb_conf)
				gsn->cb_conf(type, EOF, pdp, cbp);
			/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
			   pdp_freepdp(pdp); */
			return EOF;
		}

		if (version == 0) {
			if (gtpie_gettv2(ie, GTPIE_FL_DI, 0, &pdp->flru)) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing conditional information field\n");
				if (gsn->cb_conf)
					gsn->cb_conf(type, EOF, pdp, cbp);
				/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
				   pdp_freepdp(pdp); */
				return EOF;
			}

			if (gtpie_gettv2(ie, GTPIE_FL_C, 0, &pdp->flrc)) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing conditional information field\n");
				if (gsn->cb_conf)
					gsn->cb_conf(type, EOF, pdp, cbp);
				/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
				   pdp_freepdp(pdp); */
				return EOF;
			}
		}

		if (version == 1) {
			if (gtpie_gettv4(ie, GTPIE_TEI_DI, 0, &pdp->teid_gn)) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing conditional information field\n");
				if (gsn->cb_conf)
					gsn->cb_conf(type, EOF, pdp, cbp);
				/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
				   pdp_freepdp(pdp); */
				return EOF;
			}

			if (gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn)) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing conditional information field\n");
				if (gsn->cb_conf)
					gsn->cb_conf(type, EOF, pdp, cbp);
				/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
				   pdp_freepdp(pdp); */
				return EOF;
			}
		}

		if (gtpie_gettv4(ie, GTPIE_CHARGING_ID, 0, &pdp->cid)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len,
				    "Missing conditional information field\n");
			if (gsn->cb_conf)
				gsn->cb_conf(type, EOF, pdp, cbp);
			/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
			   pdp_freepdp(pdp); */
		}

		if (gtpie_gettlv(ie, GTPIE_EUA, 0, &pdp->eua.l,
				 &pdp->eua.v, sizeof(pdp->eua.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len,
				    "Missing conditional information field\n");
			if (gsn->cb_conf)
				gsn->cb_conf(type, EOF, pdp, cbp);
			/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
			   pdp_freepdp(pdp); */
			return EOF;
		}

		if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 0, &pdp->gsnrc.l,
				 &pdp->gsnrc.v, sizeof(pdp->gsnrc.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len,
				    "Missing conditional information field\n");
			if (gsn->cb_conf)
				gsn->cb_conf(type, EOF, pdp, cbp);
			/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
			   pdp_freepdp(pdp); */
			return EOF;
		}

		if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 1, &pdp->gsnru.l,
				 &pdp->gsnru.v, sizeof(pdp->gsnru.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len,
				    "Missing conditional information field\n");
			if (gsn->cb_conf)
				gsn->cb_conf(type, EOF, pdp, cbp);
			/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
			   pdp_freepdp(pdp); */
			return EOF;
		}

		if (version == 1) {
			if (gtpie_gettlv
			    (ie, GTPIE_QOS_PROFILE, 0, &pdp->qos_neg.l,
			     &pdp->qos_neg.v, sizeof(pdp->qos_neg.v))) {
				gsn->missing++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len,
					    "Missing conditional information field\n");
				if (gsn->cb_conf)
					gsn->cb_conf(type, EOF, pdp, cbp);
				/*    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
				   pdp_freepdp(pdp); */
				return EOF;
			}
		}

	}

	if (gsn->cb_conf)
		gsn->cb_conf(type, cause, pdp, cbp);

	return 0;
}

/* API: Send Update PDP Context Request */
int gtp_update_context(struct gsn_t *gsn, struct pdp_t *pdp, void *cbp,
		       struct in_addr *inetaddr)
{
	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(pdp->version, GTP_UPDATE_PDP_REQ, &packet);

	if (pdp->version == 0)
		gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE0,
			  sizeof(pdp->qos_req0), pdp->qos_req0);

	/* Include IMSI if updating with unknown teic_gn */
	if ((pdp->version == 1) && (!pdp->teic_gn))
		gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_IMSI,
			  sizeof(pdp->imsi), (uint8_t *) & pdp->imsi);

	gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_RECOVERY,
		  gsn->restart_counter);

	if (pdp->version == 0) {
		gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_DI, pdp->fllu);
		gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_C, pdp->fllc);
	}

	if (pdp->version == 1) {
		gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_DI,
			  pdp->teid_own);

		if (!pdp->teic_confirmed)
			gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_C,
				  pdp->teic_own);
	}

	gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_NSAPI, pdp->nsapi);

	/* TODO
	   gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_TRACE_REF,
	   pdp->traceref);
	   gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_TRACE_TYPE,
	   pdp->tracetype); */

	/* TODO if ggsn update message
	   gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA,
	   pdp->eua.l, pdp->eua.v);
	 */

	gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
		  pdp->gsnlc.l, pdp->gsnlc.v);
	gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
		  pdp->gsnlu.l, pdp->gsnlu.v);

	if (pdp->version == 1)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE,
			  pdp->qos_req.l, pdp->qos_req.v);

	if ((pdp->version == 1) && pdp->tft.l)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_TFT,
			  pdp->tft.l, pdp->tft.v);

	if ((pdp->version == 1) && pdp->triggerid.l)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_TRIGGER_ID,
			  pdp->triggerid.l, pdp->triggerid.v);

	if ((pdp->version == 1) && pdp->omcid.l)
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_OMC_ID,
			  pdp->omcid.l, pdp->omcid.v);

	gtp_req(gsn, pdp->version, pdp, &packet, length, inetaddr, cbp);

	return 0;
}

/* Send Update PDP Context Response */
static int gtp_update_pdp_resp(struct gsn_t *gsn, uint8_t version,
			struct sockaddr_in *peer, int fd,
			void *pack, unsigned len,
			struct pdp_t *pdp, uint8_t cause)
{

	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(version, GTP_UPDATE_PDP_RSP, &packet);

	gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_CAUSE, cause);

	if (cause == GTPCAUSE_ACC_REQ) {

		if (version == 0)
			gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE0,
				  sizeof(pdp->qos_neg0), pdp->qos_neg0);

		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_RECOVERY,
			  gsn->restart_counter);

		if (version == 0) {
			gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_DI,
				  pdp->fllu);
			gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_FL_C,
				  pdp->fllc);
		}

		if (version == 1) {
			gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_DI,
				  pdp->teid_own);

			if (!pdp->teic_confirmed)
				gtpie_tv4(&packet, &length, GTP_MAX,
					  GTPIE_TEI_C, pdp->teic_own);
		}

		/* TODO we use teid_own as charging ID address */
		gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_CHARGING_ID,
			  pdp->teid_own);

		/* If ggsn
		   gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA,
		   pdp->eua.l, pdp->eua.v); */

		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
			  pdp->gsnlc.l, pdp->gsnlc.v);
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
			  pdp->gsnlu.l, pdp->gsnlu.v);

		if (version == 1)
			gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE,
				  pdp->qos_neg.l, pdp->qos_neg.v);

		/* TODO: Charging gateway address */
	}

	return gtp_resp(version, gsn, pdp, &packet, length, peer,
			fd, get_seq(pack), get_tid(pack));
}

/* Handle Update PDP Context Request */
static int gtp_update_pdp_ind(struct gsn_t *gsn, uint8_t version,
		       struct sockaddr_in *peer, int fd,
		       void *pack, unsigned len)
{
	struct pdp_t *pdp;
	struct pdp_t pdp_backup;
	union gtpie_member *ie[GTPIE_SIZE];
	uint8_t recovery;

	uint16_t seq = get_seq(pack);
	int hlen = get_hlen(pack);

	uint64_t imsi;
	uint8_t nsapi;

	/* Is this a duplicate ? */
	if (!gtp_duplicate(gsn, version, peer, seq)) {
		return 0;	/* We allready send of response once */
	}

	/* Decode information elements */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		if (0 == version)
			return EOF;
		else
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, NULL,
						   GTPCAUSE_INVALID_MESSAGE);
	}

	/* Finding PDP: */
	/* For GTP0 we use the tunnel identifier to provide imsi and nsapi. */
	/* For GTP1 we must use imsi and nsapi if imsi is present. Otherwise */
	/* we have to use the tunnel endpoint identifier */
	if (version == 0) {
		uint64_t tid = be64toh(((union gtp_packet *)pack)->gtp0.h.tid);

		pdp_set_imsi_nsapi(pdp, tid);

		/* Find the context in question */
		if (pdp_getimsi(&pdp, imsi, nsapi)) {
			gsn->err_unknownpdp++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Unknown PDP context\n");
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, NULL,
						   GTPCAUSE_NON_EXIST);
		}
	} else if (version == 1) {
		/* NSAPI (mandatory) */
		if (gtpie_gettv1(ie, GTPIE_NSAPI, 0, &nsapi)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, NULL,
						   GTPCAUSE_MAN_IE_MISSING);
		}

		/* IMSI (conditional) */
		if (gtpie_gettv0(ie, GTPIE_IMSI, 0, &imsi, sizeof(imsi))) {
			/* Find the context in question */
			if (pdp_getgtp1(&pdp, get_tei(pack))) {
				gsn->err_unknownpdp++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					pack, len, "Unknown PDP context: %u\n",
					get_tei(pack));
				return gtp_update_pdp_resp(gsn, version, peer,
							   fd, pack, len, NULL,
							   GTPCAUSE_NON_EXIST);
			}
		} else {
			/* Find the context in question */
			if (pdp_getimsi(&pdp, imsi, nsapi)) {
				gsn->err_unknownpdp++;
				GTP_LOGPKG(LOGL_ERROR, peer,
					    pack, len, "Unknown PDP context\n");
				return gtp_update_pdp_resp(gsn, version, peer,
							   fd, pack, len, NULL,
							   GTPCAUSE_NON_EXIST);
			}
		}
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown version: %d\n", version);
		return EOF;
	}

	/* Make a backup copy in case anything is wrong */
	memcpy(&pdp_backup, pdp, sizeof(pdp_backup));

	if (version == 0) {
		if (gtpie_gettv0(ie, GTPIE_QOS_PROFILE0, 0,
				 pdp->qos_req0, sizeof(pdp->qos_req0))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
	}

	/* Recovery (optional) */
	if (!gtpie_gettv1(ie, GTPIE_RECOVERY, 0, &recovery)) {
		emit_cb_recovery(gsn, peer, pdp, recovery);
	}

	if (version == 0) {
		if (gtpie_gettv2(ie, GTPIE_FL_DI, 0, &pdp->flru)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}

		if (gtpie_gettv2(ie, GTPIE_FL_C, 0, &pdp->flrc)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
	}

	if (version == 1) {
		/* TEID (mandatory) */
		if (gtpie_gettv4(ie, GTPIE_TEI_DI, 0, &pdp->teid_gn)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}

		/* TEIC (conditional) */
		/* If TEIC is not included it means that we have allready received it */
		/* TODO: From 29.060 it is not clear if TEI_C MUST be included for */
		/* all updated contexts, or only for one of the linked contexts */
		gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn);

		/* NSAPI (mandatory) */
		if (gtpie_gettv1(ie, GTPIE_NSAPI, 0, &pdp->nsapi)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}
	}

	/* Trace reference (optional) */
	/* Trace type (optional) */

	/* End User Address (conditional) TODO: GGSN Initiated
	   if (gtpie_gettlv(ie, GTPIE_EUA, 0, &pdp->eua.l,
	   &pdp->eua.v, sizeof(pdp->eua.v))) {
	   gsn->missing++;
	   GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
	   "Missing mandatory information field");
	   memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
	   return gtp_update_pdp_resp(gsn, version, pdp,
	   GTPCAUSE_MAN_IE_MISSING);
	   } */

	/* SGSN address for signalling (mandatory) */
	/* It is weird that this is mandatory when TEIC is conditional */
	if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 0, &pdp->gsnrc.l,
			 &pdp->gsnrc.v, sizeof(pdp->gsnrc.v))) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory information field\n");
		memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
		return gtp_update_pdp_resp(gsn, version, peer, fd, pack, len,
					   pdp, GTPCAUSE_MAN_IE_MISSING);
	}

	/* SGSN address for user traffic (mandatory) */
	if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 1, &pdp->gsnru.l,
			 &pdp->gsnru.v, sizeof(pdp->gsnru.v))) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory information field\n");
		memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
		return gtp_update_pdp_resp(gsn, version, peer, fd, pack, len,
					   pdp, GTPCAUSE_MAN_IE_MISSING);
	}

	if (version == 1) {
		/* QoS (mandatory) */
		if (gtpie_gettlv(ie, GTPIE_QOS_PROFILE, 0, &pdp->qos_req.l,
				 &pdp->qos_req.v, sizeof(pdp->qos_req.v))) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			memcpy(pdp, &pdp_backup, sizeof(pdp_backup));
			return gtp_update_pdp_resp(gsn, version, peer, fd, pack,
						   len, pdp,
						   GTPCAUSE_MAN_IE_MISSING);
		}

		/* TFT (conditional) */
		if (gtpie_gettlv(ie, GTPIE_TFT, 0, &pdp->tft.l,
				 &pdp->tft.v, sizeof(pdp->tft.v))) {
		}

		/* OMC identity */
	}

	/* Confirm to peer that things were "successful" */
	return gtp_update_pdp_resp(gsn, version, peer, fd, pack, len, pdp,
				   GTPCAUSE_ACC_REQ);
}

/* Handle Update PDP Context Response */
static int gtp_update_pdp_conf(struct gsn_t *gsn, uint8_t version,
			struct sockaddr_in *peer, void *pack, unsigned len)
{
	struct pdp_t *pdp;
	union gtpie_member *ie[GTPIE_SIZE];
	uint8_t cause, recovery;
	void *cbp = NULL;
	uint8_t type = 0;
	int hlen = get_hlen(pack);

	/* Remove packet from queue */
	if (gtp_conf(gsn, 0, peer, pack, len, &type, &cbp))
		return EOF;

	/* Find the context in question */
	if (pdp_getgtp1(&pdp, get_tei(pack))) {
		gsn->err_unknownpdp++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Unknown PDP context: %u\n", get_tei(pack));
		pdp = NULL;
		goto err_out;
	}

	/* Register that we have received a valid teic from GGSN */
	pdp->teic_confirmed = 1;

	/* Decode information elements */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		goto err_out;
	}

	/* Extract cause value (mandatory) */
	if (gtpie_gettv1(ie, GTPIE_CAUSE, 0, &cause)) {
		goto err_missing;
	}

	/* Extract recovery (optional) */
	if (!gtpie_gettv1(ie, GTPIE_RECOVERY, 0, &recovery)) {
		emit_cb_recovery(gsn, peer, pdp, recovery);
	}

	/* Check all conditional information elements */
	/* TODO: This does not handle GGSN-initiated update responses */
	if (GTPCAUSE_ACC_REQ == cause) {
		if (version == 0) {
			if (gtpie_gettv0(ie, GTPIE_QOS_PROFILE0, 0,
					 &pdp->qos_neg0,
					 sizeof(pdp->qos_neg0))) {
				goto err_missing;
			}

			if (gtpie_gettv2(ie, GTPIE_FL_DI, 0, &pdp->flru)) {
				goto err_missing;
			}

			if (gtpie_gettv2(ie, GTPIE_FL_C, 0, &pdp->flrc)) {
				goto err_missing;
			}
		}

		if (version == 1) {
			if (gtpie_gettv4(ie, GTPIE_TEI_DI, 0, &pdp->teid_gn)) {
				goto err_missing;
			}

			if (gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn)) {
				goto err_missing;
			}
		}

		if (gtpie_gettv4(ie, GTPIE_CHARGING_ID, 0, &pdp->cid)) {
			goto err_missing;
		}

		if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 0, &pdp->gsnrc.l,
				 &pdp->gsnrc.v, sizeof(pdp->gsnrc.v))) {
			goto err_missing;
		}

		if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 1, &pdp->gsnru.l,
				 &pdp->gsnru.v, sizeof(pdp->gsnru.v))) {
			goto err_missing;
		}

		if (version == 1) {
			if (gtpie_gettlv
			    (ie, GTPIE_QOS_PROFILE, 0, &pdp->qos_neg.l,
			     &pdp->qos_neg.v, sizeof(pdp->qos_neg.v))) {
				goto err_missing;
			}
		}
	}

	if (gsn->cb_conf)
		gsn->cb_conf(type, cause, pdp, cbp);
	return 0;	/* Succes */

err_missing:
	gsn->missing++;
	GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
		    "Missing information field\n");
err_out:
	if (gsn->cb_conf)
		gsn->cb_conf(type, EOF, pdp, cbp);
	return EOF;
}

/* API: Deprecated. Send Delete PDP Context Request And free pdp ctx. */
int gtp_delete_context_req(struct gsn_t *gsn, struct pdp_t *pdp, void *cbp,
			   int teardown)
{
	struct pdp_t *linked_pdp;

	if (pdp_getgtp1(&linked_pdp, pdp->teic_own)) {
		LOGP(DLGTP, LOGL_ERROR,
			"Unknown linked PDP context: %u\n", pdp->teic_own);
		return EOF;
	}

	if (gtp_delete_context_req2(gsn, pdp, cbp, teardown) == EOF)
		return EOF;

	if (teardown) {		/* Remove all contexts */
		gtp_freepdp_teardown(gsn, linked_pdp);
	} else {
		if (gsn->cb_delete_context)
			gsn->cb_delete_context(pdp);
		if (pdp == linked_pdp) {
			linked_pdp->secondary_tei[pdp->nsapi & 0xf0] = 0;
			linked_pdp->nodata = 1;
		} else
			pdp_freepdp(pdp);
	}

	return 0;
}

/* API: Send Delete PDP Context Request. PDP CTX shall be free'd by user at cb_conf(GTP_DELETE_PDP_RSP) */
int gtp_delete_context_req2(struct gsn_t *gsn, struct pdp_t *pdp, void *cbp,
			   int teardown)
{
	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(pdp->version, GTP_DELETE_PDP_REQ, &packet);
	struct in_addr addr;
	struct pdp_t *linked_pdp;
	int count;

	if (gsna2in_addr(&addr, &pdp->gsnrc)) {
		gsn->err_address++;
		LOGP(DLGTP, LOGL_ERROR, "GSN address (len=%u) conversion failed\n", pdp->gsnrc.l);
		return EOF;
	}

	if (pdp_getgtp1(&linked_pdp, pdp->teic_own)) {
		LOGP(DLGTP, LOGL_ERROR,
			"Unknown linked PDP context: %u\n", pdp->teic_own);
		return EOF;
	}

	if (!teardown) {
		count = pdp_count_secondary(linked_pdp);
		if (count <= 1) {
			LOGP(DLGTP, LOGL_ERROR,
				"Must use teardown for last context: %d\n", count);
			return EOF;
		}
	}

	if (pdp->version == 1) {
		if (teardown)
			gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_TEARDOWN,
				  0xff);

		gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_NSAPI, pdp->nsapi);
	}

	gtp_req(gsn, pdp->version, pdp, &packet, length, &addr, cbp);

	return 0;
}

/* Send Delete PDP Context Response */
int gtp_delete_pdp_resp(struct gsn_t *gsn, int version,
			struct sockaddr_in *peer, int fd,
			void *pack, unsigned len,
			struct pdp_t *pdp, struct pdp_t *linked_pdp,
			uint8_t cause, int teardown)
{
	union gtp_packet packet;
	unsigned int length =
	    get_default_gtp(version, GTP_DELETE_PDP_RSP, &packet);

	gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_CAUSE, cause);

	gtp_resp(version, gsn, pdp, &packet, length, peer, fd,
		 get_seq(pack), get_tid(pack));

	if (cause == GTPCAUSE_ACC_REQ) {
		if ((teardown) || (version == 0)) {	/* Remove all contexts */
			gtp_freepdp_teardown(gsn, linked_pdp);
		} else {	/* Remove only current context */
			if (gsn->cb_delete_context)
				gsn->cb_delete_context(pdp);
			if (pdp == linked_pdp) {
				linked_pdp->secondary_tei[pdp->nsapi & 0xf0] =
				    0;
				linked_pdp->nodata = 1;
			} else
				pdp_freepdp(pdp);
		}
	}
	/* if (cause == GTPCAUSE_ACC_REQ) */
	return 0;
}

/* Handle Delete PDP Context Request */
int gtp_delete_pdp_ind(struct gsn_t *gsn, int version,
		       struct sockaddr_in *peer, int fd,
		       void *pack, unsigned len)
{
	struct pdp_t *pdp = NULL;
	struct pdp_t *linked_pdp = NULL;
	union gtpie_member *ie[GTPIE_SIZE];

	uint16_t seq = get_seq(pack);
	int hlen = get_hlen(pack);

	uint8_t nsapi;
	uint8_t teardown = 0;
	int count;

	/* Is this a duplicate ? */
	if (!gtp_duplicate(gsn, version, peer, seq)) {
		return 0;	/* We allready send off response once */
	}

	/* Find the linked context in question */
	if (pdp_getgtp1(&linked_pdp, get_tei(pack))) {
		gsn->err_unknownpdp++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Unknown PDP context: %u\n", get_tei(pack));
		return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len,
					   NULL, NULL, GTPCAUSE_NON_EXIST,
					   teardown);
	}

	/* If version 0 this is also the secondary context */
	if (version == 0)
		pdp = linked_pdp;

	/* Decode information elements */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		if (0 == version)
			return EOF;
		else
			return gtp_delete_pdp_resp(gsn, version, peer, fd, pack,
						   len, NULL, NULL,
						   GTPCAUSE_INVALID_MESSAGE,
						   teardown);
	}

	if (version == 1) {
		/* NSAPI (mandatory) */
		if (gtpie_gettv1(ie, GTPIE_NSAPI, 0, &nsapi)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Missing mandatory information field\n");
			return gtp_delete_pdp_resp(gsn, version, peer, fd, pack,
						   len, NULL, NULL,
						   GTPCAUSE_MAN_IE_MISSING,
						   teardown);
		}

		/* Find the context in question */
		if (pdp_getgtp1(&pdp, linked_pdp->secondary_tei[nsapi & 0x0f])) {
			gsn->err_unknownpdp++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Unknown PDP context\n");
			return gtp_delete_pdp_resp(gsn, version, peer, fd, pack,
						   len, NULL, NULL,
						   GTPCAUSE_NON_EXIST,
						   teardown);
		}

		/* Teardown (conditional) */
		gtpie_gettv1(ie, GTPIE_TEARDOWN, 0, &teardown);

		if (!teardown) {
			/* TS 29.060 section 7.3.5: If a GSN receives a Delete PDP context
			 * without a Teardown Indicator or with a Teardown Indicator with
			 * value set to "0" and only that PDP context is active for a PDN
			 * connection, then the GSN shall ignore the message.  (Note:
			 * This is symptom of a race condition. The reliable delivery of
			 * signalling messages will eventually lead to a consistent
			 * situation, allowing the teardown of the PDP context.)
			 */
			count = pdp_count_secondary(linked_pdp);
			if (count <= 1) {
				GTP_LOGPKG(LOGL_NOTICE, peer, pack, len,
					   "Ignoring CTX DEL without teardown and count=%d\n",
					   count);
				return 0;	/* 29.060 7.3.5 Ignore message */
			}
		}
	}

	return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len,
				   pdp, linked_pdp, GTPCAUSE_ACC_REQ, teardown);
}

/* Handle Delete PDP Context Response */
int gtp_delete_pdp_conf(struct gsn_t *gsn, int version,
			struct sockaddr_in *peer, void *pack, unsigned len)
{
	union gtpie_member *ie[GTPIE_SIZE];
	uint8_t cause;
	void *cbp = NULL;
	uint8_t type = 0;
	struct pdp_t *pdp = NULL;
	int hlen = get_hlen(pack);

	/* Remove packet from queue */
	if (gtp_conf(gsn, version, peer, pack, len, &type, &cbp))
		return EOF;

	/* Find the context in question. It may not be available if gtp_delete_context_req
	 * was used and as a result the PDP ctx was already freed */
	if (pdp_getgtp1(&pdp, get_tei(pack))) {
		gsn->err_unknownpdp++;
		GTP_LOGPKG(LOGL_NOTICE, peer, pack, len,
			    "Unknown PDP context: %u (expected if gtp_delete_context_req is used)\n",
			     get_tei(pack));
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, NULL, cbp);
		return EOF;
	}

	/* Decode information elements */
	if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
		gsn->invalid++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Invalid message format\n");
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, pdp, cbp);
		return EOF;
	}

	/* Extract cause value (mandatory) */
	if (gtpie_gettv1(ie, GTPIE_CAUSE, 0, &cause)) {
		gsn->missing++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Missing mandatory information field\n");
		if (gsn->cb_conf)
			gsn->cb_conf(type, EOF, pdp, cbp);
		return EOF;
	}

	/* Check the cause value (again) */
	if ((GTPCAUSE_ACC_REQ != cause) && (GTPCAUSE_NON_EXIST != cause)) {
		gsn->err_cause++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Unexpected cause value received: %d\n", cause);
		if (gsn->cb_conf)
			gsn->cb_conf(type, cause, pdp, cbp);
		return EOF;
	}

	/* Callback function to notify application */
	if (gsn->cb_conf)
		gsn->cb_conf(type, cause, pdp, cbp);

	return 0;
}

/* Send Error Indication (response to a GPDU message) - 3GPP TS 29.060 7.3.7 */
static int gtp_error_ind_resp(struct gsn_t *gsn, uint8_t version,
		       struct sockaddr_in *peer, int fd,
		       void *pack, unsigned len)
{
	union gtp_packet packet;
	unsigned int length = get_default_gtp(version, GTP_ERROR, &packet);

	if (version == 1) {
		/* Mandatory 7.7.13 TEI Data I */
		gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_DI,
			  ntoh32(((union gtp_packet *)pack)->gtp1l.h.tei));

		/* Mandatory 7.7.32 GSN Address */
		gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_GSN_ADDR,
			  sizeof(gsn->gsnu), &gsn->gsnu);
	}

	return gtp_resp(version, gsn, NULL, &packet, length, peer, fd,
			get_seq(pack), get_tid(pack));
}

/* Handle Error Indication */
static int gtp_error_ind_conf(struct gsn_t *gsn, uint8_t version,
		       struct sockaddr_in *peer, void *pack, unsigned len)
{
	union gtpie_member *ie[GTPIE_SIZE];
	struct pdp_t *pdp;

	/* Find the context in question */
	if (version == 0) {
		if (pdp_tidget(&pdp, be64toh(((union gtp_packet *)pack)->gtp0.h.tid))) {
			gsn->err_unknownpdp++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
				    "Unknown PDP context\n");
			return EOF;
		}
	} else if (version == 1) {
		/* we have to look-up based on the *peer* TEID */
		int hlen = get_hlen(pack);
		uint32_t teid_gn;

		/* Decode information elements */
		if (gtpie_decaps(ie, version, pack + hlen, len - hlen)) {
			gsn->invalid++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
				    "Invalid message format\n");
			return EOF;
		}

		if (gtpie_gettv4(ie, GTPIE_TEI_DI, 0, &teid_gn)) {
			gsn->missing++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
				    "Missing mandatory information field\n");
			return EOF;
		}

		if (pdp_getgtp1_peer_d(&pdp, peer, teid_gn)) {
			gsn->err_unknownpdp++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack, len, "Unknown PDP context\n");
			return EOF;
		}
	}

	GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
		    "Received Error Indication\n");

	/* This is obvious from above code, given the semantics of the
	 * functions above, but Coverity doesn't figure this out, so
	 * let's make it clear. It's good style anyway in case above
	 * code should ever change. */
	OSMO_ASSERT(pdp);

	gtp_freepdp(gsn, pdp);
	return 0;
}

static int gtp_gpdu_ind(struct gsn_t *gsn, uint8_t version,
		 struct sockaddr_in *peer, int fd, void *pack, unsigned len)
{

	int hlen;

	struct pdp_t *pdp;

	switch (version) {
	case 0:
		if (pdp_getgtp0
		    (&pdp, ntoh16(((union gtp_packet *)pack)->gtp0.h.flow))) {
			gsn->err_unknownpdp++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Unknown PDP context, GTPv0\n");
			return gtp_error_ind_resp(gsn, version, peer, fd, pack,
						  len);
		}
		hlen = GTP0_HEADER_SIZE;
		break;
	case 1:
		if (pdp_getgtp1
		    (&pdp, ntoh32(((union gtp_packet *)pack)->gtp1l.h.tei))) {
			gsn->err_unknownpdp++;
			GTP_LOGPKG(LOGL_ERROR, peer, pack,
				    len, "Unknown PDP context, GTPv1\n");
			return gtp_error_ind_resp(gsn, version, peer, fd, pack,
						  len);
		}

		/* Is this a long or a short header ? */
		if (((union gtp_packet *)pack)->gtp1l.h.flags & 0x07)
			hlen = GTP1_HEADER_SIZE_LONG;
		else
			hlen = GTP1_HEADER_SIZE_SHORT;
		break;
	default:
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len,
			    "Unknown version: %d\n", version);
		return EOF;
	}

	/* If the GPDU was not from the peer GSN tell him to delete context */
	if (memcmp(&peer->sin_addr, pdp->gsnru.v, pdp->gsnru.l)) {	/* TODO Range? */
		gsn->err_unknownpdp++;
		GTP_LOGPKG(LOGL_ERROR, peer, pack, len, "Unknown GSN peer %s\n", inet_ntoa(peer->sin_addr));
		return gtp_error_ind_resp(gsn, version, peer, fd, pack, len);
	}

	/* Callback function */
	if (gsn->cb_data_ind != 0)
		return gsn->cb_data_ind(pdp, pack + hlen, len - hlen);

	return 0;
}

/* Receives GTP packet and sends off for further processing
 * Function will check the validity of the header. If the header
 * is not valid the packet is either dropped or a version not
 * supported is returned to the peer.
 * TODO: Need to decide on return values! */
int gtp_decaps0(struct gsn_t *gsn)
{
	unsigned char buffer[PACKET_MAX];
	struct sockaddr_in peer;
	socklen_t peerlen;
	int status;
	struct gtp0_header *pheader;
	uint8_t version;
	int fd = gsn->fd0;

	/* TODO: Need strategy of userspace buffering and blocking */
	/* Currently read is non-blocking and send is blocking. */
	/* This means that the program have to wait for busy send calls... */

	while (1) {		/* Loop until no more to read */
		if (fcntl(gsn->fd0, F_SETFL, O_NONBLOCK)) {
			LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
			return -1;
		}
		peerlen = sizeof(peer);
		if ((status =
		     recvfrom(gsn->fd0, buffer, sizeof(buffer), 0,
			      (struct sockaddr *)&peer, &peerlen)) < 0) {
			if (errno == EAGAIN)
				return 0;
			gsn->err_readfrom++;
			LOGP(DLGTP, LOGL_ERROR,
				"recvfrom(fd0=%d, buffer=%lx, len=%zu) failed: status = %d error = %s\n",
				gsn->fd0, (unsigned long)buffer, sizeof(buffer),
				status, status ? strerror(errno) : "No error");
			return -1;
		}

		/* Need at least 1 byte in order to check version */
		if (status < (1)) {
			gsn->empty++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Discarding packet - too small\n");
			continue;
		}

		pheader = (struct gtp0_header *)(buffer);

		version = GTPHDR_F_GET_VER(pheader->flags);

		/* Version should be gtp0 (or earlier) */
		/* 09.60 is somewhat unclear on this issue. On gsn->fd0 we expect only */
		/* GTP 0 messages. If other version message is received we reply that we */
		/* only support version 0, implying that this is the only version */
		/* supported on this port */
		if (version > 0) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unsupported GTP version %"PRIu8"\n", version);
			gtp_unsup_req(gsn, 0, &peer, gsn->fd0, buffer, status);	/* 29.60: 11.1.1 */
			continue;
		}

		/* Check length of gtp0 packet */
		if (status < GTP0_HEADER_SIZE) {
			gsn->tooshort++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "GTP0 packet too short\n");
			continue;	/* Silently discard 29.60: 11.1.2 */
		}

		/* Check packet length field versus length of packet */
		if (status != (ntoh16(pheader->length) + GTP0_HEADER_SIZE)) {
			gsn->tooshort++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "GTP packet length field does not match actual length\n");
			continue;	/* Silently discard */
		}

		if ((gsn->mode == GTP_MODE_GGSN) &&
		    ((pheader->type == GTP_CREATE_PDP_RSP) ||
		     (pheader->type == GTP_UPDATE_PDP_RSP))) {
			gsn->unexpect++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "Unexpected GTPv0 Signalling Message '%s'\n",
				    get_value_string(gtp_type_names, pheader->type));
			continue;	/* Silently discard 29.60: 11.1.4 */
		}

		if ((gsn->mode == GTP_MODE_SGSN) &&
		    ((pheader->type == GTP_CREATE_PDP_REQ) ||
		     (pheader->type == GTP_UPDATE_PDP_REQ))) {
			gsn->unexpect++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "Unexpected GTPv0 Signalling Message '%s'\n",
				    get_value_string(gtp_type_names, pheader->type));
			continue;	/* Silently discard 29.60: 11.1.4 */
		}

		switch (pheader->type) {
		case GTP_ECHO_REQ:
			gtp_echo_ind(gsn, version, &peer, fd, buffer, status);
			break;
		case GTP_ECHO_RSP:
			gtp_echo_conf(gsn, version, &peer, buffer, status);
			break;
		case GTP_NOT_SUPPORTED:
			gtp_unsup_ind(gsn, &peer, buffer, status);
			break;
		case GTP_CREATE_PDP_REQ:
			gtp_create_pdp_ind(gsn, version, &peer, fd, buffer,
					   status);
			break;
		case GTP_CREATE_PDP_RSP:
			gtp_create_pdp_conf(gsn, version, &peer, buffer,
					    status);
			break;
		case GTP_UPDATE_PDP_REQ:
			gtp_update_pdp_ind(gsn, version, &peer, fd, buffer,
					   status);
			break;
		case GTP_UPDATE_PDP_RSP:
			gtp_update_pdp_conf(gsn, version, &peer, buffer,
					    status);
			break;
		case GTP_DELETE_PDP_REQ:
			gtp_delete_pdp_ind(gsn, version, &peer, fd, buffer,
					   status);
			break;
		case GTP_DELETE_PDP_RSP:
			gtp_delete_pdp_conf(gsn, version, &peer, buffer,
					    status);
			break;
		case GTP_ERROR:
			gtp_error_ind_conf(gsn, version, &peer, buffer, status);
			break;
		case GTP_GPDU:
			gtp_gpdu_ind(gsn, version, &peer, fd, buffer, status);
			break;
		default:
			gsn->unknown++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unknown GTP message type received: %d\n",
				pheader->type);
			break;
		}
	}
}

int gtp_decaps1c(struct gsn_t *gsn)
{
	unsigned char buffer[PACKET_MAX];
	struct sockaddr_in peer;
	socklen_t peerlen;
	int status;
	struct gtp1_header_short *pheader;
	uint8_t version;
	int fd = gsn->fd1c;

	/* TODO: Need strategy of userspace buffering and blocking */
	/* Currently read is non-blocking and send is blocking. */
	/* This means that the program have to wait for busy send calls... */

	while (1) {		/* Loop until no more to read */
		if (fcntl(fd, F_SETFL, O_NONBLOCK)) {
			LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
			return -1;
		}
		peerlen = sizeof(peer);
		if ((status =
		     recvfrom(fd, buffer, sizeof(buffer), 0,
			      (struct sockaddr *)&peer, &peerlen)) < 0) {
			if (errno == EAGAIN)
				return 0;
			gsn->err_readfrom++;
			LOGP(DLGTP, LOGL_ERROR,
				"recvfrom(fd=%d, buffer=%lx, len=%zu) failed: status = %d error = %s\n",
				fd, (unsigned long)buffer, sizeof(buffer),
				status, status ? strerror(errno) : "No error");
			return -1;
		}

		/* Need at least 1 byte in order to check version */
		if (status < (1)) {
			gsn->empty++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Discarding packet - too small\n");
			continue;
		}

		pheader = (struct gtp1_header_short *)(buffer);

		version = GTPHDR_F_GET_VER(pheader->flags);

		/* Version must be no larger than GTP 1 */
		if (version > 1) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unsupported GTP version %"PRIu8"\n", version);
			gtp_unsup_req(gsn, version, &peer, fd, buffer, status);
			/*29.60: 11.1.1 */
			continue;
		}

		/* Version must be at least GTP 1 */
		/* 29.060 is somewhat unclear on this issue. On gsn->fd1c we expect only */
		/* GTP 1 messages. If GTP 0 message is received we silently discard */
		/* the message */
		if (version < 1) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unsupported GTP version %"PRIu8"\n", version);
			continue;
		}

		/* Check packet flag field */
		if (((pheader->flags & 0xf7) != 0x32)) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Unsupported packet flags: 0x%02x\n", pheader->flags);
			continue;
		}

		/* Check length of packet */
		if (status < GTP1_HEADER_SIZE_LONG) {
			gsn->tooshort++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "GTP packet too short\n");
			continue;	/* Silently discard 29.60: 11.1.2 */
		}

		/* Check packet length field versus length of packet */
		if (status !=
		    (ntoh16(pheader->length) + GTP1_HEADER_SIZE_SHORT)) {
			gsn->tooshort++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "GTP packet length field does not match actual length\n");
			continue;	/* Silently discard */
		}

		/* Check for extension headers */
		/* TODO: We really should cycle through the headers and determine */
		/* if any have the comprehension required flag set */
		if (((pheader->flags & GTP1HDR_F_EXT) != 0x00)) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Unsupported extension header\n");
			gtp_extheader_req(gsn, version, &peer, fd, buffer,
					  status);

			continue;
		}

		if ((gsn->mode == GTP_MODE_GGSN) &&
		    ((pheader->type == GTP_CREATE_PDP_RSP) ||
		     (pheader->type == GTP_UPDATE_PDP_RSP))) {
			gsn->unexpect++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "Unexpected GTPv1 Signalling Message '%s'\n",
				    get_value_string(gtp_type_names, pheader->type));
			continue;	/* Silently discard 29.60: 11.1.4 */
		}

		if ((gsn->mode == GTP_MODE_SGSN) &&
		    ((pheader->type == GTP_CREATE_PDP_REQ) ||
		     (pheader->type == GTP_UPDATE_PDP_REQ))) {
			gsn->unexpect++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "Unexpected GTPv1 Signalling Message '%s'\n",
				    get_value_string(gtp_type_names, pheader->type));
			continue;	/* Silently discard 29.60: 11.1.4 */
		}

		switch (pheader->type) {
		case GTP_ECHO_REQ:
			gtp_echo_ind(gsn, version, &peer, fd, buffer, status);
			break;
		case GTP_ECHO_RSP:
			gtp_echo_conf(gsn, version, &peer, buffer, status);
			break;
		case GTP_NOT_SUPPORTED:
			gtp_unsup_ind(gsn, &peer, buffer, status);
			break;
		case GTP_SUPP_EXT_HEADER:
			gtp_extheader_ind(gsn, &peer, buffer, status);
			break;
		case GTP_CREATE_PDP_REQ:
			gtp_create_pdp_ind(gsn, version, &peer, fd, buffer,
					   status);
			break;
		case GTP_CREATE_PDP_RSP:
			gtp_create_pdp_conf(gsn, version, &peer, buffer,
					    status);
			break;
		case GTP_UPDATE_PDP_REQ:
			gtp_update_pdp_ind(gsn, version, &peer, fd, buffer,
					   status);
			break;
		case GTP_UPDATE_PDP_RSP:
			gtp_update_pdp_conf(gsn, version, &peer, buffer,
					    status);
			break;
		case GTP_DELETE_PDP_REQ:
			gtp_delete_pdp_ind(gsn, version, &peer, fd, buffer,
					   status);
			break;
		case GTP_DELETE_PDP_RSP:
			gtp_delete_pdp_conf(gsn, version, &peer, buffer,
					    status);
			break;
		case GTP_ERROR:
			gtp_error_ind_conf(gsn, version, &peer, buffer, status);
			break;
		default:
			gsn->unknown++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unknown GTP message type received: %u\n",
				pheader->type);
			break;
		}
	}
}

int gtp_decaps1u(struct gsn_t *gsn)
{
	unsigned char buffer[PACKET_MAX];
	struct sockaddr_in peer;
	socklen_t peerlen;
	int status;
	struct gtp1_header_short *pheader;
	uint8_t version;
	int fd = gsn->fd1u;

	/* TODO: Need strategy of userspace buffering and blocking */
	/* Currently read is non-blocking and send is blocking. */
	/* This means that the program have to wait for busy send calls... */

	while (1) {		/* Loop until no more to read */
		if (fcntl(gsn->fd1u, F_SETFL, O_NONBLOCK)) {
			LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
			return -1;
		}
		peerlen = sizeof(peer);
		if ((status =
		     recvfrom(gsn->fd1u, buffer, sizeof(buffer), 0,
			      (struct sockaddr *)&peer, &peerlen)) < 0) {
			if (errno == EAGAIN)
				return 0;
			gsn->err_readfrom++;
			LOGP(DLGTP, LOGL_ERROR,
				"recvfrom(fd1u=%d, buffer=%lx, len=%zu) failed: status = %d error = %s\n",
				gsn->fd1u, (unsigned long)buffer,
				sizeof(buffer), status,
				status ? strerror(errno) : "No error");
			return -1;
		}

		/* Need at least 1 byte in order to check version */
		if (status < (1)) {
			gsn->empty++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Discarding packet - too small\n");
			continue;
		}

		pheader = (struct gtp1_header_short *)(buffer);

		version = GTPHDR_F_GET_VER(pheader->flags);

		/* Version must be no larger than GTP 1 */
		if (version > 1) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unsupported GTP version %"PRIu8"\n", version);
			gtp_unsup_req(gsn, 1, &peer, gsn->fd1c, buffer, status);	/*29.60: 11.1.1 */
			continue;
		}

		/* Version must be at least GTP 1 */
		/* 29.060 is somewhat unclear on this issue. On gsn->fd1c we expect only */
		/* GTP 1 messages. If GTP 0 message is received we silently discard */
		/* the message */
		if (version < 1) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unsupported GTP version %"PRIu8"\n", version);
			continue;
		}

		/* Check packet flag field (allow both with and without sequence number) */
		if (((pheader->flags & 0xf5) != 0x30)) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Unsupported packet flags 0x%02x\n", pheader->flags);
			continue;
		}

		/* Check length of packet */
		if (status < GTP1_HEADER_SIZE_SHORT) {
			gsn->tooshort++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "GTP packet too short\n");
			continue;	/* Silently discard 29.60: 11.1.2 */
		}

		/* Check packet length field versus length of packet */
		if (status !=
		    (ntoh16(pheader->length) + GTP1_HEADER_SIZE_SHORT)) {
			gsn->tooshort++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status,
				    "GTP packet length field does not match actual length\n");
			continue;	/* Silently discard */
		}

		/* Check for extension headers */
		/* TODO: We really should cycle through the headers and determine */
		/* if any have the comprehension required flag set */
		if (((pheader->flags & GTP1HDR_F_EXT) != 0x00)) {
			gsn->unsup++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer,
				    status, "Unsupported extension header\n");
			gtp_extheader_req(gsn, version, &peer, fd, buffer,
					  status);

			continue;
		}

		switch (pheader->type) {
		case GTP_ECHO_REQ:
			gtp_echo_ind(gsn, version, &peer, fd, buffer, status);
			break;
		case GTP_ECHO_RSP:
			gtp_echo_conf(gsn, version, &peer, buffer, status);
			break;
		case GTP_SUPP_EXT_HEADER:
			gtp_extheader_ind(gsn, &peer, buffer, status);
			break;
		case GTP_ERROR:
			gtp_error_ind_conf(gsn, version, &peer, buffer, status);
			break;
			/* Supported header extensions */
		case GTP_GPDU:
			gtp_gpdu_ind(gsn, version, &peer, fd, buffer, status);
			break;
		default:
			gsn->unknown++;
			GTP_LOGPKG(LOGL_ERROR, &peer, buffer, status,
				"Unknown GTP message type received: %u\n",
				pheader->type);
			break;
		}
	}
}

int gtp_data_req(struct gsn_t *gsn, struct pdp_t *pdp, void *pack, unsigned len)
{
	union gtp_packet packet;
	struct sockaddr_in addr;
	struct msghdr msgh;
	struct iovec iov[2];
	int fd;

	/* prepare destination address */
	memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
#if defined(__FreeBSD__) || defined(__APPLE__)
	addr.sin_len = sizeof(addr);
#endif
	memcpy(&addr.sin_addr, pdp->gsnru.v, pdp->gsnru.l);	/* TODO range check */

	/* prepare msghdr */
	memset(&msgh, 0, sizeof(msgh));
	msgh.msg_name = &addr;
	msgh.msg_namelen = sizeof(addr);
	msgh.msg_iov = iov;
	msgh.msg_iovlen = ARRAY_SIZE(iov);

	/* prepare iovectors */
	iov[0].iov_base = &packet;
	/* iov[0].iov_len is not known here yet */
	iov[1].iov_base = pack;
	iov[1].iov_len = len;

	if (pdp->version == 0) {

		iov[0].iov_len = GTP0_HEADER_SIZE;
		addr.sin_port = htons(GTP0_PORT);
		fd = gsn->fd0;

		get_default_gtp(0, GTP_GPDU, &packet);
		packet.gtp0.h.length = hton16(len);
		if (pdp->tx_gpdu_seq)
			packet.gtp0.h.seq = hton16(pdp->gtpsntx++);
		else
			packet.gtp0.h.seq = 0;
		packet.gtp0.h.flow = hton16(pdp->flru);
		packet.gtp0.h.tid = htobe64(pdp_gettid(pdp->imsi, pdp->nsapi));
	} else if (pdp->version == 1) {

		addr.sin_port = htons(GTP1U_PORT);
		fd = gsn->fd1u;

		get_default_gtp(1, GTP_GPDU, &packet);
		if (pdp->tx_gpdu_seq) {
			packet.gtp1l.h.seq = hton16(pdp->gtpsntx++);
			packet.gtp1l.h.length = hton16(len - GTP1_HEADER_SIZE_SHORT +
						       GTP1_HEADER_SIZE_LONG);
			packet.gtp1l.h.tei = hton32(pdp->teid_gn);
			iov[0].iov_len = GTP1_HEADER_SIZE_LONG;
		} else {
			packet.gtp1s.h.flags &= ~GTP1HDR_F_SEQ;
			packet.gtp1s.h.length = hton16(len);
			packet.gtp1s.h.tei = hton32(pdp->teid_gn);
			iov[0].iov_len = GTP1_HEADER_SIZE_SHORT;
		}
	} else {
		LOGP(DLGTP, LOGL_ERROR, "Unknown version: %d\n", pdp->version);
		return EOF;
	}

	if (fcntl(fd, F_SETFL, 0)) {
		LOGP(DLGTP, LOGL_ERROR, "fnctl()\n");
		return -1;
	}

	if (sendmsg(fd, &msgh, 0) < 0) {
		gsn->err_sendto++;
		LOGP(DLGTP, LOGL_ERROR,
			"sendmsg(fd=%d, msg=%lx, len=%d) failed: Error = %s\n", fd,
			(unsigned long)&packet, GTP0_HEADER_SIZE + len,
			strerror(errno));
		return EOF;
	}
	return 0;
}

/* ***********************************************************
 * Conversion functions
 *************************************************************/

/* ***********************************************************
 * IP address conversion functions
 * There exist several types of address representations:
 * - eua: End User Address. (29.060, 7.7.27, message type 128)
 *   Used for signalling address to mobile station. Supports IPv4
 *   IPv6 x.25 etc. etc.
 * - gsna: GSN Address. (29.060, 7.7.32, message type 133): IP address
 *   of GSN. If length is 4 it is IPv4. If length is 16 it is IPv6.
 * - in_addr: IPv4 address struct.
 * - sockaddr_in: Socket API representation of IP address and
 *   port number.
 *************************************************************/

int ipv42eua(struct ul66_t *eua, struct in_addr *src)
{
	eua->v[0] = PDP_EUA_ORG_IETF;
	eua->v[1] = PDP_EUA_TYPE_v4;
	if (src) {
		eua->l = 6;
		memcpy(&eua->v[2], src, 4);
	} else {
		eua->l = 2;
	}
	return 0;
}

int eua2ipv4(struct in_addr *dst, struct ul66_t *eua)
{
	if ((eua->l != 6) || (eua->v[0] != PDP_EUA_ORG_IETF) || (eua->v[1] != PDP_EUA_TYPE_v4))
		return -1;	/* Not IPv4 address */
	memcpy(dst, &eua->v[2], 4);
	return 0;
}

int gsna2in_addr(struct in_addr *dst, struct ul16_t *gsna)
{
	memset(dst, 0, sizeof(struct in_addr));
	if (gsna->l != 4)
		return EOF;	/* Return if not IPv4 */
	memcpy(dst, gsna->v, gsna->l);
	return 0;
}

int in_addr2gsna(struct ul16_t *gsna, struct in_addr *src)
{
	memset(gsna, 0, sizeof(struct ul16_t));
	gsna->l = 4;
	memcpy(gsna->v, src, gsna->l);
	return 0;
}

/* TS 29.060 has yet again a different encoding for IMSIs than
 * what we have in other places, so we cannot use the gsm48
 * decoding functions.  Also, libgtp uses an uint64_t in
 * _network byte order_ to contain BCD digits ?!? */
const char *imsi_gtp2str(const uint64_t *imsi)
{
	static char buf[sizeof(*imsi)*2+1];
	const uint8_t *imsi8 = (const uint8_t *) imsi;
	unsigned int i, j = 0;

	for (i = 0; i < sizeof(*imsi); i++) {
		uint8_t nibble;

		nibble = imsi8[i] & 0xf;
		if (nibble == 0xf)
			break;
		buf[j++] = osmo_bcd2char(nibble);

		nibble = imsi8[i] >> 4;
		if (nibble == 0xf)
			break;
		buf[j++] = osmo_bcd2char(nibble);
	}

	buf[j++] = '\0';
	return buf;
}