diff --git a/gtp/Makefile.am b/gtp/Makefile.am
index 88131cc..871a446 100644
--- a/gtp/Makefile.am
+++ b/gtp/Makefile.am
@@ -1,6 +1,6 @@
 lib_LTLIBRARIES = libgtp.la
 
-CFLAGS = -O2 -fno-builtin -Wall -ansi -DSBINDIR='"$(sbindir)"'
+CFLAGS = -O2 -fno-builtin -Wall -ansi -DSBINDIR='"$(sbindir)"' -ggdb
 
 libgtp_la_SOURCES = gtp.c gtp.h gtpie.c gtpie.h pdp.c pdp.h lookupa.c lookupa.h queue.c queue.h
 
diff --git a/gtp/Makefile.in b/gtp/Makefile.in
index 9093710..661c7dc 100644
--- a/gtp/Makefile.in
+++ b/gtp/Makefile.in
@@ -87,7 +87,7 @@
 install_sh = @install_sh@
 lib_LTLIBRARIES = libgtp.la
 
-CFLAGS = -O2 -fno-builtin -Wall -ansi -DSBINDIR='"$(sbindir)"'
+CFLAGS = -O2 -fno-builtin -Wall -ansi -DSBINDIR='"$(sbindir)"' -ggdb
 
 libgtp_la_SOURCES = gtp.c gtp.h gtpie.c gtpie.h pdp.c pdp.h lookupa.c lookupa.h queue.c queue.h
 subdir = gtp
diff --git a/gtp/gtp.c b/gtp/gtp.c
index a6aebb9..fb98be1 100644
--- a/gtp/gtp.c
+++ b/gtp/gtp.c
@@ -54,10 +54,6 @@
 #include "gtpie.h"
 #include "queue.h"
 
-
-struct gtp0_header gtp0_default;
-struct gtp1_header_long gtp1_default;
-
 /* API Functions */
 
 const char* gtp_version()
@@ -162,18 +158,29 @@
 }
 
 
-
 void get_default_gtp(int version, 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:
-    memcpy(packet, &gtp0_default, sizeof(gtp0_default));
+    /* Initialise "standard" GTP0 header */
+    memset(gtp0_default, 0, sizeof(gtp0_default));
+    gtp0_default->flags=0x1e;
+    gtp0_default->spare1=0xff;
+    gtp0_default->spare2=0xff;
+    gtp0_default->spare3=0xff;
+    gtp0_default->number=0xff;
   break;
   case 1:
-    memcpy(packet, &gtp1_default, sizeof(gtp1_default));
+    /* Initialise "standard" GTP1 header */
+    memset(gtp1_default, 0, sizeof(gtp1_default));
+    gtp0_default->flags=0x1e;
     break;
   }
 }
 
+
+
 int print_packet(void *packet, unsigned len)
 {
   int i;
@@ -448,6 +455,11 @@
 	 ntohs(peer->sin_port));
   print_packet(packet, len); 
   */
+
+  if (fcntl(gsn->fd, F_SETFL, 0)) {
+    gtp_err(LOG_ERR, __FILE__, __LINE__, "fnctl()");
+    return -1;
+  }
   
   if (sendto(gsn->fd, packet, len, 0,
 	     (struct sockaddr *) peer, sizeof(struct sockaddr_in)) < 0) {
@@ -487,6 +499,12 @@
 	   ntohs(peer->sin_port));
     print_packet(&qmsg->p, qmsg->l);
     */
+
+    if (fcntl(gsn->fd, F_SETFL, 0)) {
+      gtp_err(LOG_ERR, __FILE__, __LINE__, "fnctl()");
+      return -1;
+    }
+
     if (sendto(gsn->fd, &qmsg->p, qmsg->l, 0,
 	       (struct sockaddr *) peer, sizeof(struct sockaddr_in)) < 0) {
       gsn->err_sendto++;
@@ -553,6 +571,9 @@
 
   (*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);
@@ -573,8 +594,6 @@
     return -1;
   }
   (*gsn)->fd = gtp_fd;
-  
-  /* syslog(LOG_ERR, "GTP: gtp_init() after socket");*/
 
   (*gsn)->gsnc = *listen;
   (*gsn)->gsnu = *listen;
@@ -592,18 +611,6 @@
     return -1;
   }
 
-  /* Initialise "standard" GTP0 header */
-  memset(&gtp0_default, 0, sizeof(gtp0_default));
-  gtp0_default.flags=0x1e;
-  gtp0_default.spare1=0xff;
-  gtp0_default.spare2=0xff;
-  gtp0_default.spare3=0xff;
-  gtp0_default.number=0xff;
-  
-  /* Initialise "standard" GTP1 header */
-  memset(&gtp1_default, 0, sizeof(gtp1_default));
-  gtp0_default.flags=0x1e;
-  
   return 0;
 }
 
@@ -1736,90 +1743,111 @@
   struct gtp0_header *pheader;
   int version = 0; /* GTP version should be determined from header!*/
 
-  peerlen = sizeof(peer);
-  if ((status = 
-       recvfrom(gsn->fd, buffer, sizeof(buffer), 0,
-		(struct sockaddr *) &peer, &peerlen)) < 0 ) {
-    gsn->err_readfrom++;
-    gtp_err(LOG_ERR, __FILE__, __LINE__, "recvfrom(fd=%d, buffer=%lx, len=%d) failed: status = %d error = %s", gsn->fd, (unsigned long) buffer, sizeof(buffer), status, status ? strerror(errno) : "No error");
-    return -1;
-  }
-  
-  /* Strip off IP header, if present: TODO Is this nessesary? */
-  if ((buffer[0] & 0xF0) == 0x40) {
-    ip_len = (buffer[0] & 0xF) * 4;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
-		"IP header found in return from read");
-    return -1;
-  }
-  
-  /* Need at least 1 byte in order to check version */
-  if (status < (1)) {
-    gsn->empty++;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
-		"Discarding packet - too small");
-    return -1;
-  }
-  
-  /* TODO: Remove these ERROR MESSAGES 
-  gtp_err(LOG_ERR, __FILE__, __LINE__, "Discarding packet - too small");
-  gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
-	      "Discarding packet - too small"); */
+  /* 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...*/
 
-  pheader = (struct gtp0_header *) (buffer + ip_len);
-
-  /* Version should be gtp0 (or earlier in theory) */
-  if (((pheader->flags & 0xe0) > 0x00)) {
-    gsn->unsup++;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
-		"Unsupported GTP version");
-    return gtp_unsup_resp(gsn, &peer, buffer, status); /* 29.60: 11.1.1 */
-  }
+  while (1) { /* Loop until no more to read */
+    if (fcntl(gsn->fd, F_SETFL, O_NONBLOCK)) {
+      gtp_err(LOG_ERR, __FILE__, __LINE__, "fnctl()");
+      return -1;
+    }
+    peerlen = sizeof(peer);
+    if ((status = 
+	 recvfrom(gsn->fd, buffer, sizeof(buffer), 0,
+		  (struct sockaddr *) &peer, &peerlen)) < 0 ) {
+      if (errno == EAGAIN) return -1;
+      gsn->err_readfrom++;
+      gtp_err(LOG_ERR, __FILE__, __LINE__, "recvfrom(fd=%d, buffer=%lx, len=%d) failed: status = %d error = %s", gsn->fd, (unsigned long) buffer, sizeof(buffer), status, status ? strerror(errno) : "No error");
+      return -1;
+    }
   
-  /* Check length of gtp0 packet */
-  if (((pheader->flags & 0xe0) == 0x00) && (status < GTP0_HEADER_SIZE)) {
-    gsn->tooshort++;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
-		"GTP0 packet too short");
-    return -1; /* Silently discard 29.60: 11.1.2 */
-  }
-
-  switch (pheader->type) {
-  case GTP_ECHO_REQ:
-    return gtp_echo_ind(gsn, &peer, buffer+ip_len, status - ip_len);
-  case GTP_ECHO_RSP:
-    return gtp_echo_conf(gsn, &peer, buffer+ip_len, status - ip_len);
-  case GTP_NOT_SUPPORTED:
-    return gtp_unsup_conf(gsn, &peer, buffer+ip_len, status - ip_len);
-  case GTP_CREATE_PDP_REQ:
-    return gtp_create_pdp_ind(gsn, version, &peer, buffer+ip_len, 
-			      status - ip_len);
-  case GTP_CREATE_PDP_RSP:
-    return gtp_create_pdp_conf(gsn, version, &peer, buffer+ip_len, 
-			       status - ip_len);
-  case GTP_UPDATE_PDP_REQ:
-    return gtp_update_pdp_ind(gsn, version, &peer, buffer+ip_len, 
-			      status - ip_len);
-  case GTP_UPDATE_PDP_RSP:
-    return gtp_update_pdp_conf(gsn, version, &peer, buffer+ip_len, 
-			       status - ip_len);
-  case GTP_DELETE_PDP_REQ:
-    return gtp_delete_pdp_ind(gsn, version, &peer, buffer+ip_len, 
-			      status - ip_len);
-  case GTP_DELETE_PDP_RSP:
-    return gtp_delete_pdp_conf(gsn, version, &peer, buffer+ip_len, 
-			       status - ip_len);
-  case GTP_ERROR:
-    return gtp_error_ind_conf(gsn, version, &peer, buffer+ip_len, 
-			      status - ip_len);
-  case GTP_GPDU:
-    return gtp_gpdu_ind(gsn, version, &peer, buffer+ip_len, status - ip_len);
-  default:
-    {
+    /* Strip off IP header, if present: TODO Is this nessesary? */
+    if ((buffer[0] & 0xF0) == 0x40) {
+      ip_len = (buffer[0] & 0xF) * 4;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+		  "IP header found in return from read");
+      continue;
+    }
+    
+    /* Need at least 1 byte in order to check version */
+    if (status < (1)) {
+      gsn->empty++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+		  "Discarding packet - too small");
+      continue;
+    }
+    
+    /* TODO: Remove these ERROR MESSAGES 
+       gtp_err(LOG_ERR, __FILE__, __LINE__, "Discarding packet - too small");
+       gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+       "Discarding packet - too small"); */
+    
+    pheader = (struct gtp0_header *) (buffer + ip_len);
+    
+    /* Version should be gtp0 (or earlier in theory) */
+    if (((pheader->flags & 0xe0) > 0x00)) {
+      gsn->unsup++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+		  "Unsupported GTP version");
+      gtp_unsup_resp(gsn, &peer, buffer, status); /* 29.60: 11.1.1 */
+      continue;
+    }
+    
+    /* Check length of gtp0 packet */
+    if (((pheader->flags & 0xe0) == 0x00) && (status < GTP0_HEADER_SIZE)) {
+      gsn->tooshort++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+		  "GTP0 packet too short");
+      continue;  /* Silently discard 29.60: 11.1.2 */
+    }
+    
+    switch (pheader->type) {
+    case GTP_ECHO_REQ:
+      gtp_echo_ind(gsn, &peer, buffer+ip_len, status - ip_len);
+      break;
+    case GTP_ECHO_RSP:
+      gtp_echo_conf(gsn, &peer, buffer+ip_len, status - ip_len);
+      break;
+    case GTP_NOT_SUPPORTED:
+      gtp_unsup_conf(gsn, &peer, buffer+ip_len, status - ip_len);
+      break;
+    case GTP_CREATE_PDP_REQ:
+      gtp_create_pdp_ind(gsn, version, &peer, buffer+ip_len, 
+			 status - ip_len);
+      break;
+    case GTP_CREATE_PDP_RSP:
+      gtp_create_pdp_conf(gsn, version, &peer, buffer+ip_len, 
+			  status - ip_len);
+      break;
+    case GTP_UPDATE_PDP_REQ:
+      gtp_update_pdp_ind(gsn, version, &peer, buffer+ip_len, 
+			 status - ip_len);
+      break;
+    case GTP_UPDATE_PDP_RSP:
+      gtp_update_pdp_conf(gsn, version, &peer, buffer+ip_len, 
+			  status - ip_len);
+      break;
+    case GTP_DELETE_PDP_REQ:
+      gtp_delete_pdp_ind(gsn, version, &peer, buffer+ip_len, 
+			 status - ip_len);
+      break;
+    case GTP_DELETE_PDP_RSP:
+      gtp_delete_pdp_conf(gsn, version, &peer, buffer+ip_len, 
+			  status - ip_len);
+      break;
+    case GTP_ERROR:
+      gtp_error_ind_conf(gsn, version, &peer, buffer+ip_len, 
+			 status - ip_len);
+      break;
+    case GTP_GPDU:
+      gtp_gpdu_ind(gsn, version, &peer, buffer+ip_len, status - ip_len);
+      break;
+    default:
       gsn->unknown++;
       gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
 		  "Unknown GTP message type received");
-      return -1;
+      break;
     }
   }
 }
@@ -1853,6 +1881,11 @@
     return EOF;
     }
 
+  if (fcntl(gsn->fd, F_SETFL, 0)) {
+    gtp_err(LOG_ERR, __FILE__, __LINE__, "fnctl()");
+    return -1;
+  }
+
   memcpy(packet.gtp0.p, pack, len); /* TODO Should be avoided! */
   
   if (sendto(gsn->fd, &packet, GTP0_HEADER_SIZE+len, 0,
diff --git a/gtp/pdp.c b/gtp/pdp.c
index 276c8ac..2c05bdb 100644
--- a/gtp/pdp.c
+++ b/gtp/pdp.c
@@ -1,6 +1,6 @@
 /* 
  *  OpenGGSN - Gateway GPRS Support Node
- *  Copyright (C) 2002 Mondru AB.
+ *  Copyright (C) 2002, 2003 Mondru AB.
  * 
  *  The contents of this file may be used under the terms of the GNU
  *  General Public License Version 2, provided that the above copyright
@@ -32,7 +32,7 @@
 
 struct pdp_t pdpa[PDP_MAX];    /* PDP storage */
 struct pdp_t* hashtid[PDP_MAX];/* Hash table for IMSI + NSAPI */
-struct pdp_t* haship[PDP_MAX]; /* Hash table for IP and network interface */
+/* struct pdp_t* haship[PDP_MAX];  Hash table for IP and network interface */
 
 /* ***********************************************************
  * Functions related to PDP storage
@@ -109,7 +109,7 @@
 int pdp_init() {
   memset(&pdpa, 0, sizeof(pdpa));
   memset(&hashtid, 0, sizeof(hashtid));
-  memset(&haship, 0, sizeof(haship));
+  /*  memset(&haship, 0, sizeof(haship)); */
 
   return 0;
 }
@@ -227,8 +227,9 @@
   return EOF; /* End of linked list and not found */
 }
 
+/*
 int pdp_iphash(void* ipif, struct ul66_t *eua) {
-  /*printf("IPhash %ld\n", lookup(eua->v, eua->l, ipif) % PDP_MAX);*/
+  /#printf("IPhash %ld\n", lookup(eua->v, eua->l, ipif) % PDP_MAX);#/
   return (lookup(eua->v, eua->l, ipif) % PDP_MAX);
 }
     
@@ -276,27 +277,27 @@
     pdp_prev = pdp2;
   }
   if (PDP_DEBUG) printf("End pdp_ipdel: PDP not found\n");
-  return EOF; /* End of linked list and not found */
+  return EOF; /# End of linked list and not found #/
 }
 
 int pdp_ipget(struct pdp_t **pdp, void* ipif, struct ul66_t *eua) {
   int hash = pdp_iphash(ipif, eua);
   struct pdp_t *pdp2;
-  /*printf("Begin pdp_ipget %d %d %2x%2x%2x%2x\n", (unsigned)ipif, eua->l, 
-    eua->v[2],eua->v[3],eua->v[4],eua->v[5]);*/
+  /#printf("Begin pdp_ipget %d %d %2x%2x%2x%2x\n", (unsigned)ipif, eua->l, 
+    eua->v[2],eua->v[3],eua->v[4],eua->v[5]);#/
   for (pdp2 = haship[hash]; pdp2; pdp2 = pdp2->ipnext) {
     if ((pdp2->ipif == ipif) && (pdp2->eua.l == eua->l) && 
 	(memcmp(&pdp2->eua.v, &eua->v, eua->l) == 0)) {
       *pdp = pdp2;
-      /*printf("End pdp_ipget. Found\n");*/
+      /#printf("End pdp_ipget. Found\n");#/
       return 0;
     }
   }
   if (PDP_DEBUG) printf("End pdp_ipget Notfound %d %d %2x%2x%2x%2x\n", 
 	 (unsigned)ipif, eua->l, eua->v[2],eua->v[3],eua->v[4],eua->v[5]);
-  return EOF; /* End of linked list and not found */
+  return EOF; /# End of linked list and not found #/
 }
-
+*/
 /* Various conversion functions */
 
 int pdp_ntoeua(struct in_addr *src, struct ul66_t *eua) {
@@ -307,6 +308,14 @@
   return 0;
 }
 
+int pdp_euaton(struct ul66_t *eua, struct in_addr *dst) {
+  if((eua->l!=6) || (eua->v[0]!=0xf1) || (eua->v[1]!=0x21)) {
+    return EOF;
+  }
+  memcpy(dst, &eua->v[2], 4); /* Copy a 4 byte address */
+  return 0;
+}
+
 uint64_t pdp_gettid(uint64_t imsi, uint8_t nsapi) {
   return (imsi & 0x0fffffffffffffff) + ((uint64_t)nsapi << 60);
 }
diff --git a/gtp/pdp.h b/gtp/pdp.h
index b9ca62a..b4f9986 100644
--- a/gtp/pdp.h
+++ b/gtp/pdp.h
@@ -1,6 +1,6 @@
 /* 
  *  OpenGGSN - Gateway GPRS Support Node
- *  Copyright (C) 2002 Mondru AB.
+ *  Copyright (C) 2002, 2003 Mondru AB.
  * 
  *  The contents of this file may be used under the terms of the GNU
  *  General Public License Version 2, provided that the above copyright
@@ -98,6 +98,7 @@
   /* Parameters shared by all PDP context belonging to the same MS */
 
   void *ipif;           /* IP network interface */
+  void *peer;           /* Pointer to peer protocol */
   void *asap;           /* Application specific service access point */
 
   uint64_t    imsi;     /* International Mobile Subscriber Identity.*/
@@ -191,12 +192,15 @@
 int pdp_tiddel(struct pdp_t *pdp);
 int pdp_tidget(struct pdp_t **pdp, uint64_t tid);
 
+/*
 int pdp_iphash(void* ipif, struct ul66_t *eua);
 int pdp_ipset(struct pdp_t *pdp, void* ipif, struct ul66_t *eua);
 int pdp_ipdel(struct pdp_t *pdp);
 int pdp_ipget(struct pdp_t **pdp, void* ipif, struct ul66_t *eua);
+*/
 
 int pdp_ntoeua(struct in_addr *src, struct ul66_t *eua);
+int pdp_euaton(struct ul66_t *eua, struct in_addr *dst);
 uint64_t pdp_gettid(uint64_t imsi, uint8_t nsapi);
 int ulcpy(void* dst, void* src, size_t size);
 
diff --git a/gtp/queue.c b/gtp/queue.c
index 0080e43..900f240 100644
--- a/gtp/queue.c
+++ b/gtp/queue.c
@@ -133,7 +133,8 @@
     (*qmsg)->this = queue->next;
     (*qmsg)->next=-1;       /* End of the queue */
     (*qmsg)->prev=queue->last; /* Link to the previous */
-    queue->qmsga[queue->last].next=queue->next; /* Link previous to us */
+    if (queue->last != -1)
+      queue->qmsga[queue->last].next=queue->next; /* Link previous to us */
     queue->last = queue->next;                  /* End of queue */
     if (queue->first == -1) queue->first = queue->next;
     queue->next = (queue->next+1) % QUEUE_SIZE;   /* Increment */