GTP1 functionality

5 files changed, 586 insertions, 242 deletions

diff --git a/gtp/gtp.c b/gtp/gtp.c
index 27d8a03..6f94aa5 100644
--- a/gtp/gtp.c
+++ b/gtp/gtp.c
@@ -139,6 +139,12 @@ int gtp_set_cb_unsup_ind(struct gsn_t *gsn,
   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 */
@@ -383,6 +389,9 @@ char* snprint_packet(struct gsn_t *gsn, struct sockaddr_in *peer,
  * 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.
@@ -596,6 +605,50 @@ int gtp_resp(int version, struct gsn_t *gsn, struct pdp_t *pdp,
   return 0;
 }
 
+int gtp_notification(struct gsn_t *gsn, int version,
+		     union gtp_packet *packet, int len,
+		     struct sockaddr_in *peer, int fd, 
+		     uint16_t seq) {
+
+  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 ((packet->flags & 0xe0) == 0x00) { /* Version 0 */
+    packet->gtp0.h.length = hton16(len - GTP0_HEADER_SIZE);
+    packet->gtp0.h.seq = hton16(seq);
+  }
+  else if ((packet->flags & 0xe2) == 0x22) { /* Version 1 with seq */
+    packet->gtp1l.h.length = hton16(len - GTP1_HEADER_SIZE_SHORT);
+    packet->gtp1l.h.seq = hton16(seq);
+  }
+  else {
+    gtp_err(LOG_ERR, __FILE__, __LINE__, "Unknown packet flag");
+    return -1;
+  }
+  
+  if (fcntl(fd, F_SETFL, 0)) {
+    gtp_err(LOG_ERR, __FILE__, __LINE__, "fnctl()");
+    return -1;
+  }
+
+  if (sendto(fd, packet, len, 0,
+	     (struct sockaddr *) &addr, sizeof(struct sockaddr_in)) < 0) {
+    gsn->err_sendto++;
+    gtp_err(LOG_ERR, __FILE__, __LINE__, "Sendto(fd=%d, msg=%lx, len=%d) failed: Error = %s", fd, (unsigned long) &packet, len, strerror(errno));
+    return -1;
+  }
+  return 0;
+}
+
 int gtp_dublicate(struct gsn_t *gsn, int version,  
 		  struct sockaddr_in *peer, uint16_t seq) {
   struct qmsg_t *qmsg;
@@ -884,8 +937,9 @@ int gtp_unsup_req(struct gsn_t *gsn, int version, struct sockaddr_in *peer,
   union gtp_packet packet;
 
   /* GTP 1 is the highest supported protocol */
-  int hlen = get_default_gtp(1, GTP_NOT_SUPPORTED, &packet);
-  return gtp_resp(version, gsn, NULL, &packet, hlen, peer, fd, 0, 0);
+  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 */
@@ -897,6 +951,36 @@ int gtp_unsup_ind(struct gsn_t *gsn, struct sockaddr_in *peer,
   return 0;
 }
 
+/* Send off an Supported Extension Headers Notification */
+int gtp_extheader_req(struct gsn_t *gsn, int version, struct sockaddr_in *peer,
+		      int fd, void *pack, unsigned len)
+{
+  union gtp_packet packet;
+  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 */
+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
@@ -914,19 +998,38 @@ extern int gtp_create_context_req(struct gsn_t *gsn, struct pdp_t *pdp,
 				  void *cbp, struct in_addr* inetaddr) {
   union gtp_packet packet;
   int length = get_default_gtp(pdp->version, GTP_CREATE_PDP_REQ, &packet);
+  struct pdp_t *linked_pdp = NULL;
 
-  if (pdp->version == 0)
+  /* 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)) {
+      gtp_err(LOG_ERR, __FILE__, __LINE__, "Unknown linked PDP context");
+      return EOF;
+    }
+  }
+
+  if (pdp->version == 0) {
     gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE0, 
 	      sizeof(pdp->qos_req0), pdp->qos_req0);
+  }
 
-  if (pdp->version == 1)
-    gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_IMSI, 
-	      sizeof(pdp->imsi), (uint8_t*) &pdp->imsi);
+  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);
+  }
 
   gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_RECOVERY, 
 	    gsn->restart_counter);
-  gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_SELECTION_MODE,
-	    pdp->selmode);
+
+  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, 
@@ -938,17 +1041,21 @@ extern int gtp_create_context_req(struct gsn_t *gsn, struct pdp_t *pdp,
   if (pdp->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);
+
+    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);
 
-  /*gtpie_tv1(packet.gtp1l.p, &length, GTP_MAX, GTPIE_NSAPI, 
-    pdp->nsapil); For use by several QoS profiles for the same address */
 
   if (pdp->version == 1) {
+    if (pdp->secondary) /* Secondary PDP Context Activation Procedure */
+      gtpie_tv1(packet.gtp1l.p, &length, GTP_MAX, GTPIE_NSAPI, 
+		linked_pdp->nsapi);
+    
     gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_CHARGING_C,
 	      pdp->cch_pdp);
   }
@@ -959,21 +1066,28 @@ extern int gtp_create_context_req(struct gsn_t *gsn, struct pdp_t *pdp,
   gtpie_tv2(&packet, &length, GTP_MAX, GTPIE_TRACE_TYPE,
 	    pdp->tracetype); */
 
-  gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA, 
-	    pdp->eua.l, pdp->eua.v);
-  gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_APN, 
-	    pdp->apn_use.l, pdp->apn_use.v);
+  if (!pdp->secondary) /* Not Secondary PDP Context Activation Procedure */
+    gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA, 
+	      pdp->eua.l, pdp->eua.v);
+  
 
-  if (pdp->pco_req.l)
-    gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_PCO, 
-	      pdp->pco_req.l, pdp->pco_req.v);
+  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);
+
+  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);
   
   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);
-  gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_MSISDN,
-	    pdp->msisdn.l, pdp->msisdn.v);
+
+  if (!pdp->secondary) /* Not Secondary PDP Context Activation Procedure */
+    gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_MSISDN,
+	      pdp->msisdn.l, pdp->msisdn.v);
 
   if (pdp->version == 1) 
     gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE,
@@ -997,7 +1111,6 @@ extern int gtp_create_context_req(struct gsn_t *gsn, struct pdp_t *pdp,
   return 0;
 }
 
-
 /* API: Application response to context indication */
 int gtp_create_context_resp(struct gsn_t *gsn, struct pdp_t *pdp, int cause) {
 
@@ -1053,8 +1166,10 @@ int gtp_create_pdp_resp(struct gsn_t *gsn, int version, struct pdp_t *pdp,
 		pdp->teic_own);
     }
 
+    /* TODO: We use teic_own as charging ID */
     gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_CHARGING_ID,
-	      0x12345678);
+	      pdp->teic_own);
+
     gtpie_tlv(&packet, &length, GTP_MAX, GTPIE_EUA, 
 	      pdp->eua.l, pdp->eua.v);
 
@@ -1090,8 +1205,10 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
 
   uint16_t seq = get_seq(pack);
   int hlen = get_hlen(pack);
+  uint8_t linked_nsapi = 0;
+  struct pdp_t *linked_pdp = NULL;
 
-  if(!gtp_dublicate(gsn, 0, peer, seq)) return 0;
+  if(!gtp_dublicate(gsn, version, peer, seq)) return 0;
 
   pdp = &pdp_buf;
   memset(pdp, 0, sizeof(struct pdp_t));
@@ -1117,6 +1234,43 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
       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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		    "Incorrect optional information field");
+	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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		    "Incorrect optional information field");
+	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;
+    }
+  } /* if (version == 1) */
+
   if (version == 0) {
     if (gtpie_gettv0(ie, GTPIE_QOS_PROFILE0, 0,
 		     pdp->qos_req0, sizeof(pdp->qos_req0))) {
@@ -1126,9 +1280,9 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
       return gtp_create_pdp_resp(gsn, version, pdp, GTPCAUSE_MAN_IE_MISSING);
     }
   }
-
-
-  if (version == 1) {
+  
+  if ((version == 1) && (!linked_pdp)) {
+    /* Not Secondary PDP Context Activation Procedure */
     /* IMSI (conditional) */
     if (gtpie_gettv0(ie, GTPIE_IMSI, 0, &pdp->imsi, sizeof(pdp->imsi))) {
       gsn->missing++;
@@ -1138,20 +1292,22 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
 				 GTPCAUSE_MAN_IE_MISSING);
     }
   }
-
+  
   /* Recovery (optional) */
   if (!gtpie_gettv1(ie, GTPIE_RECOVERY, 0, &recovery)) {
     /* TODO: Handle received recovery IE */
   }
-
+  
   /* Selection mode (conditional) */
-  if (gtpie_gettv0(ie, GTPIE_SELECTION_MODE, 0,
-		   &pdp->selmode, sizeof(pdp->selmode))) {
-    gsn->missing++;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-		"Missing mandatory information field");
-    return gtp_create_pdp_resp(gsn, version, pdp, 
-			       GTPCAUSE_MAN_IE_MISSING);
+  if (!linked_pdp) { /* Not Secondary PDP Context Activation Procedure */
+    if (gtpie_gettv0(ie, GTPIE_SELECTION_MODE, 0,
+		     &pdp->selmode, sizeof(pdp->selmode))) {
+      gsn->missing++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		  "Missing mandatory information field");
+      return gtp_create_pdp_resp(gsn, version, pdp, 
+				 GTPCAUSE_MAN_IE_MISSING);
+    }
   }
 
   if (version == 0) {
@@ -1171,8 +1327,8 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
 				 GTPCAUSE_MAN_IE_MISSING);
     }
   }
-
-
+  
+  
   if (version == 1) {
     /* TEID (mandatory) */
     if (gtpie_gettv4(ie, GTPIE_TEI_DI, 0, &pdp->teid_gn)) {
@@ -1184,66 +1340,59 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
     }
 
     /* TEIC (conditional) */
-    if (gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn)) {
+    if (!linked_pdp) { /* Not Secondary PDP Context Activation Procedure */
+      if (gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn)) {
+	gsn->missing++;
+	gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		    "Missing mandatory information field");
+	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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		"Missing mandatory information field");
+    return gtp_create_pdp_resp(gsn, version, pdp, 
+			       GTPCAUSE_MAN_IE_MISSING);
+  }
+  
+  
+  /* Charging Characteriatics (optional) */
+  /* Trace reference (optional) */
+  /* Trace type (optional) */
+  /* Charging Characteriatics (optional) */
+  
+  if (!linked_pdp) { /* Not Secondary PDP Context Activation Procedure */
+    /* End User Address (conditional) */
+    if (gtpie_gettlv(ie, GTPIE_EUA, 0, &pdp->eua.l,
+		     &pdp->eua.v, sizeof(pdp->eua.v))) {
       gsn->missing++;
       gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		  "Missing mandatory information field");
       return gtp_create_pdp_resp(gsn, version, pdp, 
 				 GTPCAUSE_MAN_IE_MISSING);
     }
-
-    /* NSAPI (mandatory) */
-    if (gtpie_gettv1(ie, GTPIE_NSAPI, 0, &pdp->nsapi)) {
+    
+    /* APN */
+    if (gtpie_gettlv(ie, GTPIE_APN, 0, &pdp->apn_req.l,
+		     &pdp->apn_req.v, sizeof(pdp->apn_req.v))) {
       gsn->missing++;
       gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		  "Missing mandatory information field");
       return gtp_create_pdp_resp(gsn, version, pdp, 
 				 GTPCAUSE_MAN_IE_MISSING);
     }
-
-    /* Linked NSAPI (conditional) */
-    if (gtpie_gettv1(ie, GTPIE_NSAPI, 1, &pdp->linked_nsapi)) {
-      /* TODO: Handle linked NSAPI */
-      /* Currently the Secondary PDP Context Activation Procedure is not */
-      /* supported */
-    } else {
-      gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-		  "Found Linked NSAPI");
-      return gtp_create_pdp_resp(gsn, version, pdp, 
-				 GTPCAUSE_NOT_SUPPORTED);
+    
+    /* 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))) {
     }
   }
 
-  /* Charging Characteriatics (optional) */
-  /* Trace reference (optional) */
-  /* Trace type (optional) */
-  /* Charging Characteriatics (optional) */
-
-  /* End User Address (conditional) */
-  if (gtpie_gettlv(ie, GTPIE_EUA, 0, &pdp->eua.l,
-		     &pdp->eua.v, sizeof(pdp->eua.v))) {
-    gsn->missing++;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-		"Missing mandatory information field");
-    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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-		"Missing mandatory information field");
-    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))) {
-  }
-
   /* SGSN address for signalling (mandatory) */
   if (gtpie_gettlv(ie, GTPIE_GSN_ADDR, 0, &pdp->gsnrc.l,
 		     &pdp->gsnrc.v, sizeof(pdp->gsnrc.v))) {
@@ -1264,14 +1413,16 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
 			       GTPCAUSE_MAN_IE_MISSING);
   }
   
-  /* MSISDN (conditional) */
-  if (gtpie_gettlv(ie, GTPIE_MSISDN, 0, &pdp->msisdn.l,
-		   &pdp->msisdn.v, sizeof(pdp->msisdn.v))) {
-    gsn->missing++;
-    gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-		"Missing mandatory information field");
-    return gtp_create_pdp_resp(gsn, version, pdp, 
-			       GTPCAUSE_MAN_IE_MISSING);
+  if (!linked_pdp) { /* Not Secondary PDP Context Activation Procedure */
+    /* MSISDN (conditional) */
+    if (gtpie_gettlv(ie, GTPIE_MSISDN, 0, &pdp->msisdn.l,
+		     &pdp->msisdn.v, sizeof(pdp->msisdn.v))) {
+      gsn->missing++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		  "Missing mandatory information field");
+      return gtp_create_pdp_resp(gsn, version, pdp, 
+				 GTPCAUSE_MAN_IE_MISSING);
+    }
   }
 
   if (version == 1) {
@@ -1289,7 +1440,7 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
     if (gtpie_gettlv(ie, GTPIE_TFT, 0, &pdp->tft.l,
 		     &pdp->tft.v, sizeof(pdp->tft.v))) {
     }
-    
+
     /* Trigger ID */
     /* OMC identity */
   }
@@ -1297,9 +1448,9 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
   /* Initialize our own IP addresses */
   in_addr2gsna(&pdp->gsnlc, &gsn->gsnc);
   in_addr2gsna(&pdp->gsnlu, &gsn->gsnu);
-
+  
   if (GTP_DEBUG) printf("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" */
@@ -1333,7 +1484,14 @@ int gtp_create_pdp_ind(struct gsn_t *gsn, int version,
       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;
       
       /* Confirm to peer that things were "successful" */
@@ -1386,6 +1544,9 @@ int gtp_create_pdp_conf(struct gsn_t *gsn, int version,
     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++;
@@ -1534,52 +1695,55 @@ int gtp_update_context(struct gsn_t *gsn, struct pdp_t *pdp, void *cbp,
   if (pdp->version == 0)
     gtpie_tv0(&packet, &length, GTP_MAX, GTPIE_QOS_PROFILE0, 
 	      sizeof(pdp->qos_req0), pdp->qos_req0);
-
-  if (pdp->version == 1)
+  
+  /* 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);
-    gtpie_tv4(&packet, &length, GTP_MAX, GTPIE_TEI_C,
-	      pdp->teic_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); */
-
+     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_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);
@@ -1628,31 +1792,34 @@ int gtp_update_pdp_resp(struct gsn_t *gsn, int version,
     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);
+      
+      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,
-	      0x12345678); /* TODO */
-
+	      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_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, &pdp->sa_peer, 
-		  pdp->fd, pdp->seq, pdp->tid);
+  
+  return gtp_resp(version, gsn, pdp, &packet, length, peer, 
+		  fd, get_seq(pack), get_tid(pack));
 }
 
 
@@ -1675,7 +1842,7 @@ int gtp_update_pdp_ind(struct gsn_t *gsn, int version,
   if(!gtp_dublicate(gsn, version, peer, seq)) {
     return 0; /* We allready send of response once */
   }
-
+  
   
   /* Decode information elements */
   if (gtpie_decaps(ie, version, pack+hlen, len-hlen)) {
@@ -1696,7 +1863,7 @@ int gtp_update_pdp_ind(struct gsn_t *gsn, int version,
   if (version == 0) {
     imsi = ((union gtp_packet*)pack)->gtp0.h.tid & 0x0fffffffffffffff;
     nsapi = (((union gtp_packet*)pack)->gtp0.h.tid & 0xf000000000000000) >> 60;
-
+    
     /* Find the context in question */
     if (pdp_getimsi(&pdp, imsi, nsapi)) {
       gsn->err_unknownpdp++;
@@ -1715,7 +1882,7 @@ int gtp_update_pdp_ind(struct gsn_t *gsn, int version,
       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 */
@@ -1742,7 +1909,7 @@ int gtp_update_pdp_ind(struct gsn_t *gsn, int version,
     gtp_err(LOG_ERR, __FILE__, __LINE__, "Unknown version");
     return EOF;
   }
-
+  
   /* Make a backup copy in case anything is wrong */
   memcpy(&pdp_backup, pdp, sizeof(pdp_backup));
 
@@ -1794,12 +1961,13 @@ int gtp_update_pdp_ind(struct gsn_t *gsn, int version,
       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 */
-    if (gtpie_gettv4(ie, GTPIE_TEI_C, 0, &pdp->teic_gn)) {
-    }
-
+    /* 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++;
@@ -1897,6 +2065,9 @@ int gtp_update_pdp_conf(struct gsn_t *gsn, int version,
     return EOF;
   }
 
+  /* Register that we have received a valid teic from GGSN */
+  pdp->teic_confirmed = 1;
+
   /* Decode information elements */
   if (gtpie_decaps(ie, 0, pack+GTP0_HEADER_SIZE, len-GTP0_HEADER_SIZE)) {
     gsn->invalid++;
@@ -1971,50 +2142,122 @@ int gtp_update_pdp_conf(struct gsn_t *gsn, int version,
 
 
 /* API: Send Delete PDP Context Request */
-int gtp_delete_context_req(struct gsn_t *gsn, struct pdp_t *pdp, void *cbp) {
+int gtp_delete_context_req(struct gsn_t *gsn, struct pdp_t *pdp, void *cbp,
+			   int teardown) {
   union gtp_packet packet;
-  int length = get_default_gtp(pdp->version, GTP_CREATE_PDP_REQ, &packet);
+  int length = get_default_gtp(pdp->version, GTP_DELETE_PDP_REQ, &packet);
   struct in_addr addr;
-  
+  struct pdp_t *linked_pdp;
+  struct pdp_t *secondary_pdp;
+  int n;
+  int count = 0;
+
   if (gsna2in_addr(&addr, &pdp->gsnrc)) {
     gsn->err_address++;
     gtp_err(LOG_ERR, __FILE__, __LINE__, "GSN address conversion failed");
     return EOF;
   }
-  
+
+  if (pdp_getgtp1(&linked_pdp, pdp->teic_own)) {
+    gtp_err(LOG_ERR, __FILE__, __LINE__, "Unknown linked PDP context");
+    return EOF;
+  }
+
+  if (!teardown) {
+    for (n=0; n< PDP_MAXNSAPI; n++)
+      if (linked_pdp->secondary_tei[n]) count++;
+    if (count <= 1) {
+      gtp_err(LOG_ERR, __FILE__, __LINE__, 
+	      "Must use teardown for last context");
+      return EOF;
+    }
+  }
+ 
   if (pdp->version == 1) {
     gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_NSAPI, 
 	      pdp->nsapi);
+    
+    if (teardown)
+      gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_TEARDOWN,
+		0xff);
+  }
+
+  gtp_req(gsn, pdp->version, pdp, &packet, length, &addr, cbp);
 
-    gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_TEARDOWN,
-	      0xff);
+  if (teardown) { /* Remove all contexts */
+    for (n=0; n< PDP_MAXNSAPI; n++) {
+      if (linked_pdp->secondary_tei[n]) {
+	if (pdp_getgtp1(&secondary_pdp, linked_pdp->secondary_tei[n])) {
+	  gtp_err(LOG_ERR, __FILE__, __LINE__, "Unknown secondary PDP context");
+	  return EOF;
+	}
+	if (linked_pdp != secondary_pdp) {
+	  if (gsn->cb_delete_context) gsn->cb_delete_context(secondary_pdp);
+	  pdp_freepdp(secondary_pdp);
+	}
+      }
+    }
+    if (gsn->cb_delete_context) gsn->cb_delete_context(linked_pdp);
+    pdp_freepdp(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);
   }
 
-  get_default_gtp(pdp->version, GTP_DELETE_PDP_REQ, &packet);
-  
-  return 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, uint8_t cause)
+			struct pdp_t *pdp, struct pdp_t *linked_pdp, 
+			uint8_t cause, int teardown)
 {
   union gtp_packet packet;
+  struct pdp_t *secondary_pdp;
   int length = get_default_gtp(version, GTP_DELETE_PDP_RSP, &packet);
+  int n;
 
   gtpie_tv1(&packet, &length, GTP_MAX, GTPIE_CAUSE, cause);
 
   gtp_resp(version, gsn, pdp, &packet, length, peer, fd,
-	   get_seq(pack),get_tid(pack));
+	   get_seq(pack), get_tid(pack));
 
-  if (pdp) {
-    /* Callback function to allow application to clean up */
-    if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
-    pdp_freepdp(pdp); /* Clean up PDP context */
-  }
+  if (cause == GTPCAUSE_ACC_REQ) {
+    if ((teardown) || (version == 0)) { /* Remove all contexts */
+      for (n=0; n< PDP_MAXNSAPI; n++) {
+	if (linked_pdp->secondary_tei[n]) {
+	  if (pdp_getgtp1(&secondary_pdp, linked_pdp->secondary_tei[n])) {
+	    gtp_err(LOG_ERR, __FILE__, __LINE__, 
+		    "Unknown secondary PDP context");
+	    return EOF;
+	  }
+	  if (linked_pdp != secondary_pdp) {
+	    if (gsn->cb_delete_context) gsn->cb_delete_context(secondary_pdp);
+	    pdp_freepdp(secondary_pdp);
+	  }
+	}
+      }
+      if (gsn->cb_delete_context) gsn->cb_delete_context(linked_pdp);
+      pdp_freepdp(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;
 }
@@ -2023,28 +2266,35 @@ int gtp_delete_pdp_resp(struct gsn_t *gsn, int version,
 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;
+  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;
-  
+  uint8_t teardown = 0;
+  int n;
+  int count = 0;
+
   /* Is this a dublicate ? */
   if(!gtp_dublicate(gsn, version, peer, seq)) {
     return 0; /* We allready send off response once */
   }
 
-  /* Find the context in question */
-  if (pdp_getgtp1(&pdp, get_tei(pack))) {
+  /* Find the linked context in question */
+  if (pdp_getgtp1(&linked_pdp, get_tei(pack))) {
     gsn->err_unknownpdp++;
     gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		"Unknown PDP context");
-    return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len,
-			       NULL, GTPCAUSE_NON_EXIST);
+    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)) {
@@ -2054,37 +2304,43 @@ int gtp_delete_pdp_ind(struct gsn_t *gsn, int version,
     if (0 == version)
       return EOF;
     else
-      return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len,
-				 NULL, GTPCAUSE_INVALID_MESSAGE);
+      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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		  "Missing mandatory information field");
-      return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len,
-				 NULL, GTPCAUSE_MAN_IE_MISSING);
+      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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
+		  "Unknown PDP context");
+      return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len, NULL, NULL,
+				 GTPCAUSE_NON_EXIST, teardown);
     }
-
-    /* TODO: When multiple contexts with the same IP address exists
-       we need to tear down each one individually or as a group depending
-       on the value of teardown */
     
     /* Teardown (conditional) */
-    if (gtpie_gettv1(ie, GTPIE_TEARDOWN, 0, &teardown)) {
-      return 0; /* 29.060 7.3.5 Ignore message */
-      /* gsn->missing++;
-	 gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-      "Missing mandatory information field");
-		  return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len,
-      NULL, GTPCAUSE_MAN_IE_MISSING); */
+    gtpie_gettv1(ie, GTPIE_TEARDOWN, 0, &teardown);
+
+    if (!teardown) {
+      for (n=0; n< PDP_MAXNSAPI; n++)
+	if (linked_pdp->secondary_tei[n]) count++;
+      if (count <= 1) {
+	return 0; /* 29.060 7.3.5 Ignore message */
+      }
     }
   }
-  
-  return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len, pdp,
-			     GTPCAUSE_ACC_REQ);
+
+  return gtp_delete_pdp_resp(gsn, version, peer, fd, pack, len, 
+			     pdp, linked_pdp, GTPCAUSE_ACC_REQ, teardown);
 }
 
 
@@ -2092,7 +2348,6 @@ int gtp_delete_pdp_ind(struct gsn_t *gsn, int version,
 int gtp_delete_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;
   void *cbp = NULL;
@@ -2102,21 +2357,12 @@ int gtp_delete_pdp_conf(struct gsn_t *gsn, int version,
   /* 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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
-		"Unknown PDP context");
-    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_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		"Invalid message format");
-    if (gsn->cb_conf) gsn->cb_conf(type, EOF, pdp, cbp);
+    if (gsn->cb_conf) gsn->cb_conf(type, EOF, NULL, cbp);
     return EOF;
   }
 
@@ -2125,23 +2371,21 @@ int gtp_delete_pdp_conf(struct gsn_t *gsn, int version,
     gsn->missing++;
     gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		"Missing mandatory information field");
-    if (gsn->cb_conf) gsn->cb_conf(type, EOF, pdp, cbp);
+    if (gsn->cb_conf) gsn->cb_conf(type, EOF, NULL, cbp);
     return EOF;
   }
 
-  /* Check the cause value */
-  if ((GTPCAUSE_ACC_REQ != cause) &&  (GTPCAUSE_NON_EXIST != cause)) {
+  /* Check the cause value (again) */
+  if ((GTPCAUSE_ACC_REQ != cause) && (GTPCAUSE_NON_EXIST != cause)) {
     gsn->err_cause++;
     gtp_errpack(LOG_ERR, __FILE__, __LINE__, peer, pack, len,
 		"Unexpected cause value received: %d", cause);
+    if (gsn->cb_conf) gsn->cb_conf(type, cause, NULL, cbp);
+    return EOF;
   }
-
+  
   /* Callback function to notify application */
-  if (gsn->cb_conf) gsn->cb_conf(type, cause, pdp, cbp);
-
-  /* Callback function to allow application to clean up */
-  if (gsn->cb_delete_context) gsn->cb_delete_context(pdp);
-  pdp_freepdp(pdp);  
+  if (gsn->cb_conf) gsn->cb_conf(type, cause, NULL, cbp);
   
   return 0;
 }
@@ -2242,7 +2486,7 @@ int gtp_gpdu_ind(struct gsn_t *gsn, int version,
  * TODO: Need to decide on return values! */
 int gtp_decaps0(struct gsn_t *gsn)
 {
-  unsigned char buffer[PACKET_MAX + 64 /*TODO: ip header */ ];
+  unsigned char buffer[PACKET_MAX];
   struct sockaddr_in peer;
   int peerlen;
   int status;
@@ -2374,12 +2618,12 @@ int gtp_decaps0(struct gsn_t *gsn)
 
 int gtp_decaps1c(struct gsn_t *gsn)
 {
-  unsigned char buffer[PACKET_MAX + 64 /*TODO: ip header */ ];
+  unsigned char buffer[PACKET_MAX];
   struct sockaddr_in peer;
   int peerlen;
   int status;
   struct gtp1_header_short *pheader;
-  int version = 1; /* GTP version should be determined from header!*/
+  int version = 1; /* TODO GTP version should be determined from header!*/
   int fd = gsn->fd1c;
 
   /* TODO: Need strategy of userspace buffering and blocking */
@@ -2387,17 +2631,17 @@ int gtp_decaps1c(struct gsn_t *gsn)
   /* This means that the program have to wait for busy send calls...*/
 
   while (1) { /* Loop until no more to read */
-    if (fcntl(gsn->fd1c, F_SETFL, O_NONBLOCK)) {
+    if (fcntl(fd, F_SETFL, O_NONBLOCK)) {
       gtp_err(LOG_ERR, __FILE__, __LINE__, "fnctl()");
       return -1;
     }
     peerlen = sizeof(peer);
     if ((status = 
-	 recvfrom(gsn->fd1c, buffer, sizeof(buffer), 0,
+	 recvfrom(fd, buffer, sizeof(buffer), 0,
 		  (struct sockaddr *) &peer, &peerlen)) < 0 ) {
       if (errno == EAGAIN) return 0;
       gsn->err_readfrom++;
-      gtp_err(LOG_ERR, __FILE__, __LINE__, "recvfrom(fd1c=%d, buffer=%lx, len=%d) failed: status = %d error = %s", gsn->fd1c, (unsigned long) buffer, sizeof(buffer), status, status ? strerror(errno) : "No error");
+      gtp_err(LOG_ERR, __FILE__, __LINE__, "recvfrom(fd=%d, buffer=%lx, len=%d) failed: status = %d error = %s", fd, (unsigned long) buffer, sizeof(buffer), status, status ? strerror(errno) : "No error");
       return -1;
     }
   
@@ -2416,7 +2660,8 @@ int gtp_decaps1c(struct gsn_t *gsn)
       gsn->unsup++;
       gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
 		  "Unsupported GTP version");
-      gtp_unsup_req(gsn, 1, &peer, gsn->fd1c, buffer, status);/*29.60: 11.1.1*/
+      gtp_unsup_req(gsn, version, &peer, fd, buffer, status);
+      /*29.60: 11.1.1*/
       continue;
     }
 
@@ -2455,6 +2700,18 @@ int gtp_decaps1c(struct gsn_t *gsn)
       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 & 0x04) != 0x00)) {
+      gsn->unsup++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+		  "Unsupported extension header");
+      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) ||
@@ -2485,6 +2742,9 @@ int gtp_decaps1c(struct gsn_t *gsn)
     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;
@@ -2517,7 +2777,7 @@ int gtp_decaps1c(struct gsn_t *gsn)
 
 int gtp_decaps1u(struct gsn_t *gsn)
 {
-  unsigned char buffer[PACKET_MAX + 64 /*TODO: ip header */ ];
+  unsigned char buffer[PACKET_MAX];
   struct sockaddr_in peer;
   int peerlen;
   int status;
@@ -2597,6 +2857,18 @@ int gtp_decaps1u(struct gsn_t *gsn)
 		  "GTP packet length field does not match actual length");
       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 & 0x04) != 0x00)) {
+      gsn->unsup++;
+      gtp_errpack(LOG_ERR, __FILE__, __LINE__, &peer, buffer, status,
+		  "Unsupported extension header");
+      gtp_extheader_req(gsn, version, &peer, fd, buffer, status);
+
+      continue;
+    }
     
     switch (pheader->type) {
     case GTP_ECHO_REQ:
@@ -2605,6 +2877,9 @@ int gtp_decaps1u(struct gsn_t *gsn)
     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;
@@ -2663,6 +2938,7 @@ int gtp_data_req(struct gsn_t *gsn, struct pdp_t* pdp,
     packet.gtp1l.h.length = hton16(len-GTP1_HEADER_SIZE_SHORT+
 				   GTP1_HEADER_SIZE_LONG);
     packet.gtp1l.h.seq = hton16(pdp->gtpsntx++);
+    packet.gtp1l.h.tei = hton32(pdp->teid_gn);
 
     if (len > sizeof (union gtp_packet) - sizeof(struct gtp1_header_long)) {
       gsn->err_memcpy++;
diff --git a/gtp/gtp.h b/gtp/gtp.h
index 123339b..ebe869e 100644
--- a/gtp/gtp.h
+++ b/gtp/gtp.h
@@ -38,6 +38,9 @@
 #define RESTART_FILE "gsn_restart"
 #define NAMESIZE 1024
 
+/* GTP version 1 extension header type definitions. */
+#define GTP_EXT_PDCP_PDU    0xC0 /* PDCP PDU Number */
+
 /* GTP version 1 message type definitions. Also covers version 0 except *
  * for anonymous PDP context which was superceded in version 1 */
 
@@ -259,6 +262,7 @@ struct gsn_t {
   int (*cb_delete_context) (struct pdp_t*);
   int (*cb_create_context_ind) (struct pdp_t*);
   int (*cb_unsup_ind) (struct sockaddr_in *peer);
+  int (*cb_extheader_ind) (struct sockaddr_in *peer);
   int (*cb_conf) (int type, int cause, struct pdp_t *pdp, void* cbp);
   int (*cb_data_ind) (struct pdp_t* pdp, void* pack, unsigned len);
 
@@ -282,7 +286,8 @@ struct gsn_t {
   uint64_t unknown;     /* Number of unknown messages 29.60 11.1.3 */
   uint64_t unexpect;    /* Number of unexpected messages 29.60 11.1.4 */
   uint64_t dublicate;   /* Number of dublicate or unsolicited replies */
-  uint64_t missing;     /* Number of missing mandatory field messages */
+  uint64_t missing;     /* Number of missing information field messages */
+  uint64_t incorrect;   /* Number of incorrect information field messages */
   uint64_t invalid;     /* Number of invalid message format messages */
 };
 
@@ -312,7 +317,7 @@ extern int gtp_update_context(struct gsn_t *gsn, struct pdp_t *pdp,
 			      void *cbp, struct in_addr* inetaddr);
 
 extern int gtp_delete_context_req(struct gsn_t *gsn, struct pdp_t *pdp, 
-				  void *cbp);
+				  void *cbp, int teardown);
 
 extern int gtp_data_req(struct gsn_t *gsn, struct pdp_t *pdp,
 			void *pack, unsigned len);
@@ -336,6 +341,9 @@ extern int gtp_set_cb_delete_context(struct gsn_t *gsn,
 extern int gtp_set_cb_unsup_ind(struct gsn_t *gsn,
 				int (*cb) (struct sockaddr_in *peer));
 
+extern int gtp_set_cb_extheader_ind(struct gsn_t *gsn,
+				    int (*cb) (struct sockaddr_in *peer));
+
 
 extern int gtp_set_cb_conf(struct gsn_t *gsn,
              int (*cb) (int type, int cause, struct pdp_t* pdp, void *cbp));
@@ -381,7 +389,8 @@ extern int gtp_delete_pdp_req(struct gsn_t *gsn, int version, void *cbp,
 extern 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, uint8_t cause);
+			       struct pdp_t *pdp, struct pdp_t *linked_pdp,
+			       uint8_t cause, int teardown);
 
 extern int gtp_delete_pdp_ind(struct gsn_t *gsn, int version,
 			      struct sockaddr_in *peer, int fd,
diff --git a/gtp/pdp.c b/gtp/pdp.c
index 493b9b4..130ba44 100644
--- a/gtp/pdp.c
+++ b/gtp/pdp.c
@@ -125,11 +125,18 @@ int pdp_newpdp(struct pdp_t **pdp, uint64_t imsi, uint8_t nsapi,
       (*pdp)->inuse    = 1;
       (*pdp)->imsi     = imsi;
       (*pdp)->nsapi    = nsapi;
-      (*pdp)->fllc     = (uint16_t) n;
-      (*pdp)->fllu     = (uint16_t) n;
-      (*pdp)->teic_own = (uint32_t) n;
-      (*pdp)->teid_own = (uint32_t) n;
+      (*pdp)->fllc     = (uint16_t) n + 1;
+      (*pdp)->fllu     = (uint16_t) n + 1;
+      (*pdp)->teid_own = (uint32_t) n + 1;
+      if (!(*pdp)->secondary) (*pdp)->teic_own = (uint32_t) n + 1;
       pdp_tidset(*pdp, pdp_gettid(imsi, nsapi));
+      
+      /* Insert reference in primary context */
+      if (((*pdp)->teic_own > 0 ) && ((*pdp)->teic_own <= PDP_MAX)) {
+	pdpa[(*pdp)->teic_own-1].secondary_tei[(*pdp)->nsapi & 0x0f] = 
+	  (*pdp)->teid_own;
+      }
+      
       return 0;
     }
   }
@@ -138,8 +145,13 @@ int pdp_newpdp(struct pdp_t **pdp, uint64_t imsi, uint8_t nsapi,
 
 int pdp_freepdp(struct pdp_t *pdp){
   pdp_tiddel(pdp);
+
+  /* Remove any references in primary context */
+  if ((pdp->secondary) && (pdp->teic_own > 0 ) && (pdp->teic_own <= PDP_MAX)) {
+    pdpa[pdp->teic_own-1].secondary_tei[pdp->nsapi & 0x0f] = 0;
+  }
+
   memset(pdp, 0, sizeof(struct pdp_t));
-  /* Also need to clean up IP hash tables */
   return 0;
 }
 
@@ -149,24 +161,26 @@ int pdp_getpdp(struct pdp_t **pdp){
 }
 
 int pdp_getgtp0(struct pdp_t **pdp, uint16_t fl){
-  if (fl>=PDP_MAX) {
+  if ((fl>PDP_MAX) || (fl<1)) {
     return EOF;  /* Not found */
   }
   else {
-    *pdp = &pdpa[fl];
+    *pdp = &pdpa[fl-1];
     if ((*pdp)->inuse) return 0;
     else return EOF; 
     /* Context exists. We do no further validity checking. */
   }
 }
 
-int pdp_getgtp1(struct pdp_t **pdp, uint32_t teid){
-  if (teid>=PDP_MAX) {
-    return -1;  /* Not found */
+int pdp_getgtp1(struct pdp_t **pdp, uint32_t tei){
+  if ((tei>PDP_MAX) || (tei<1)) {
+    return EOF;  /* Not found */
   }
   else {
-    *pdp = &pdpa[teid];
-    return 0; /* We do no validity checking. */
+    *pdp = &pdpa[tei-1];
+    if ((*pdp)->inuse) return 0;
+    else return EOF; 
+    /* Context exists. We do no further validity checking. */
   }
 }
 
diff --git a/gtp/pdp.h b/gtp/pdp.h
index 90a1871..4ef63b3 100644
--- a/gtp/pdp.h
+++ b/gtp/pdp.h
@@ -18,6 +18,7 @@
 #define _PDP_H
 
 #define PDP_MAX 1024             /* Max number of PDP contexts */
+#define PDP_MAXNSAPI 16          /* Max number of NSAPI */
 
 #define PDP_DEBUG 0              /* Print debug information */
 
@@ -52,19 +53,19 @@ unsigned char v[255];
 };
 
 
-/* ***********************************************************
+/* *****************************************************************
  * Information storage for each PDP context
  *
- * Information storage for each PDP context is defined in 
- * 23.060 section 13.3 and 03.60. Includes IMSI, MSISDN, APN, 
- * PDP-type, PDP-address (IP address), sequence numbers, charging ID.
- * For the SGSN it also includes radio related mobility
- * information.
- * The following structure is a combination of the storage 
- * requirements for each PDP context for the GGSN and SGSN.
- * It contains both 23.060 as well as 03.60 parameters.
- * Information is stored in the format for information elements
- * described in 29.060 and 09.60.
+ * Information storage for each PDP context is defined in 23.060
+ * section 13.3 and 03.60. Includes IMSI, MSISDN, APN, PDP-type,
+ * PDP-address (IP address), sequence numbers, charging ID.  For the
+ * SGSN it also includes radio related mobility information.
+ *
+ * The following structure is a combination of the storage
+ * requirements for each PDP context for the GGSN and SGSN.  It
+ * contains both 23.060 as well as 03.60 parameters.  Information is
+ * stored in the format for information elements described in 29.060
+ * and 09.60.
  * 31 * 4 + 15 structs +  = 120 + 15 structs ~ 2k / context
  * Structs: IP address 16+4 bytes (6), APN 255 bytes (2)
  * QOS: 255 bytes (3), msisdn 16 bytes (1), 
@@ -85,7 +86,26 @@ unsigned char v[255];
  * allocate, free, sort and find pdp_t
  * (newpdp, freepdp, getpdp)
  * Hash tables: TID, IMSI, IP etc.) 
- *************************************************************/
+ *
+ *
+ * Secondary PDP Context Activation Procedure 
+ *
+ * With GTP version 1 it is possible to establish multiple PDP
+ * contexts with the same IP address. With this scheme the first
+ * context is established as normal. Following contexts are
+ * established by referencing one of the allready existing ones.  Each
+ * context is uniquely identified by IMSI and NSAPI. Each context is
+ * allocated an tei_di, but they all share the same tei_c.
+ *
+ * For Delete PDP Context the context is referenced by tei_c and
+ * nsapi. As an option it is possible to include a teardown indicater,
+ * in which case all PDP contexts with the same tei_c (IP address) are
+ * deleted.
+ *
+ * For Update PDP Context the context is normally referenced by tei_c
+ * and nsapi. If moving from GTP0 to GTP1 during an update the context
+ * is referenced instead by IMSI and NSAPI.
+ *****************************************************************/
 
 struct pdp_t {
   /* Parameter determining if this PDP is in use. */
@@ -123,7 +143,6 @@ struct pdp_t {
   struct ul255_t apn_sub;/* The APN received from the HLR. */
   struct ul255_t apn_use;/* The APN Network Identifier currently used. */
   uint8_t     nsapi;    /* Network layer Service Access Point Identifier. (4 bit) */
-  uint8_t     linked_nsapi;  /* (Linked NSAPI) (4 bit) */
   uint16_t    ti;       /* Transaction Identifier. (4 or 12 bit) */
 
   uint32_t    teic_own; /* (Own Tunnel Endpoint Identifier Control) */
@@ -183,6 +202,16 @@ struct pdp_t {
   u_int16_t   seq;      /* Sequence number of last request */
   struct sockaddr_in sa_peer; /* Address of last request */
   int fd;               /* File descriptor request was received on */
+
+  uint8_t teic_confirmed; /* 0: Not confirmed. 1: Confirmed */
+
+  /* Parameters used for secondary activation procedure (tei data) */
+  /* If (secondary == 1) then teic_own indicates linked PDP context */
+  uint8_t secondary;    /* 0: Primary (control). 1: Secondary (data only) */
+  uint8_t nodata;       /* 0: User plane PDP context. 1: No user plane */
+
+  /* Secondary contexts of this primary context */
+  uint32_t secondary_tei[PDP_MAXNSAPI]; 
 };
 
 
diff --git a/sgsnemu/sgsnemu.c b/sgsnemu/sgsnemu.c
index 577428f..7974a90 100644
--- a/sgsnemu/sgsnemu.c
+++ b/sgsnemu/sgsnemu.c
@@ -775,9 +775,26 @@ int cb_tun_ind(struct tun_t *tun, void *pack, unsigned len) {
   return 0;
 }
 
-int create_pdp_conf(struct pdp_t *pdp, int cause) {
+int create_pdp_conf(struct pdp_t *pdp, void *cbp, int cause) {
   struct in_addr addr;
 
+  struct iphash_t *iph = (struct iphash_t*) cbp;
+
+  if (cause < 0) {
+    printf("Create PDP Context Request timed out\n");
+    if (iph->pdp->version == 1) {
+      printf("Retrying with version 0\n");
+      iph->pdp->version = 0;
+      gtp_create_context_req(gsn, iph->pdp, iph, &options.remote);
+      state = 1;  /* Enter wait_connection state */
+      return 0;
+    }
+    else {
+      state = 0;
+      return EOF;
+    }
+  }
+
   if (cause != 128) {
     printf("Received create PDP context response. Cause value: %d\n", cause);
     state = 0;
@@ -828,14 +845,13 @@ int echo_conf(int recovery) {
   return 0;
 }
 
-int conf(int type, int cause, struct pdp_t* pdp, void *aid) {
+int conf(int type, int cause, struct pdp_t* pdp, void *cbp) {
   /* if (cause < 0) return 0; Some error occurred. We don't care */
   switch (type) {
   case GTP_ECHO_REQ:
     return echo_conf(cause);
   case GTP_CREATE_PDP_REQ:
-    if (cause !=128) return 0; /* Request not accepted. We don't care */
-    return create_pdp_conf(pdp, cause);
+    return create_pdp_conf(pdp, cbp, cause);
   case GTP_DELETE_PDP_REQ:
     if (cause !=128) return 0; /* Request not accepted. We don't care */
     return delete_pdp_conf(pdp, cause);
@@ -976,7 +992,7 @@ int main(int argc, char **argv)
 
     /* Create context */
     /* We send this of once. Retransmissions are handled by gtplib */
-    gtp_create_context_req(gsn, pdp, NULL, &options.remote);
+    gtp_create_context_req(gsn, pdp, &iparr[n], &options.remote);
   }    
 
   state = 1;  /* Enter wait_connection state */
@@ -1018,14 +1034,14 @@ int main(int argc, char **argv)
 	state = 3;
     }    
 
-    /* Send of disconnect */
+    /* Send off disconnect */
     if (3 == state) {
       state = 4;
       stoptime = time(NULL) + 5; /* Extra seconds to allow disconnect */
       for(n=0; n<options.contexts; n++) {
 	/* Delete context */
 	printf("Disconnecting PDP context #%d\n", n);
-	gtp_delete_context_req(gsn, iparr[n].pdp, NULL);
+	gtp_delete_context_req(gsn, iparr[n].pdp, NULL, 1);
 	if ((options.pinghost.s_addr !=0) && ntransmitted) ping_finish();
       }
     }