Added --net, --nsapi, --gtpversion options

4 files changed, 248 insertions, 12 deletions

diff --git a/sgsnemu/cmdline.c b/sgsnemu/cmdline.c
index f6f044f..be47cc1 100644
--- a/sgsnemu/cmdline.c
+++ b/sgsnemu/cmdline.c
@@ -53,13 +53,16 @@ cmdline_parser_print_help (void)
   printf("   -rSTRING   --remote=STRING    Remote host\n");
   printf("              --contexts=INT     Number of contexts (default='1')\n");
   printf("              --timelimit=INT    Exit after timelimit seconds (default='0')\n");
+  printf("              --gtpversion=INT   GTP version to use (default='1')\n");
   printf("   -aSTRING   --apn=STRING       Access point name (default='internet')\n");
   printf("   -iSTRING   --imsi=STRING      IMSI (default='240010123456789')\n");
+  printf("              --nsapi=INT        NSAPI (default='1')\n");
   printf("   -mSTRING   --msisdn=STRING    Mobile Station ISDN number (default='46702123456')\n");
   printf("   -qINT      --qos=INT          Requested quality of service (default='0x0b921f')\n");
   printf("   -uSTRING   --uid=STRING       Login user ID (default='mig')\n");
   printf("   -pSTRING   --pwd=STRING       Login password (default='hemmelig')\n");
   printf("              --createif         Create local network interface (default=off)\n");
+  printf("   -nSTRING   --net=STRING       Network address for local interface\n");
   printf("              --defaultroute     Create default route (default=off)\n");
   printf("              --ipup=STRING      Script to run after link-up\n");
   printf("              --ipdown=STRING    Script to run after link-down\n");
@@ -102,13 +105,16 @@ cmdline_parser (int argc, char * const *argv, struct gengetopt_args_info *args_i
   args_info->remote_given = 0 ;
   args_info->contexts_given = 0 ;
   args_info->timelimit_given = 0 ;
+  args_info->gtpversion_given = 0 ;
   args_info->apn_given = 0 ;
   args_info->imsi_given = 0 ;
+  args_info->nsapi_given = 0 ;
   args_info->msisdn_given = 0 ;
   args_info->qos_given = 0 ;
   args_info->uid_given = 0 ;
   args_info->pwd_given = 0 ;
   args_info->createif_given = 0 ;
+  args_info->net_given = 0 ;
   args_info->defaultroute_given = 0 ;
   args_info->ipup_given = 0 ;
   args_info->ipdown_given = 0 ;
@@ -127,13 +133,16 @@ cmdline_parser (int argc, char * const *argv, struct gengetopt_args_info *args_i
   args_info->remote_arg = NULL; \
   args_info->contexts_arg = 1 ;\
   args_info->timelimit_arg = 0 ;\
+  args_info->gtpversion_arg = 1 ;\
   args_info->apn_arg = strdup("internet") ;\
   args_info->imsi_arg = strdup("240010123456789") ;\
+  args_info->nsapi_arg = 1 ;\
   args_info->msisdn_arg = strdup("46702123456") ;\
   args_info->qos_arg = 0x0b921f ;\
   args_info->uid_arg = strdup("mig") ;\
   args_info->pwd_arg = strdup("hemmelig") ;\
   args_info->createif_flag = 0;\
+  args_info->net_arg = NULL; \
   args_info->defaultroute_flag = 0;\
   args_info->ipup_arg = NULL; \
   args_info->ipdown_arg = NULL; \
@@ -167,13 +176,16 @@ cmdline_parser (int argc, char * const *argv, struct gengetopt_args_info *args_i
         { "remote",	1, NULL, 'r' },
         { "contexts",	1, NULL, 0 },
         { "timelimit",	1, NULL, 0 },
+        { "gtpversion",	1, NULL, 0 },
         { "apn",	1, NULL, 'a' },
         { "imsi",	1, NULL, 'i' },
+        { "nsapi",	1, NULL, 0 },
         { "msisdn",	1, NULL, 'm' },
         { "qos",	1, NULL, 'q' },
         { "uid",	1, NULL, 'u' },
         { "pwd",	1, NULL, 'p' },
         { "createif",	0, NULL, 0 },
+        { "net",	1, NULL, 'n' },
         { "defaultroute",	0, NULL, 0 },
         { "ipup",	1, NULL, 0 },
         { "ipdown",	1, NULL, 0 },
@@ -185,7 +197,7 @@ cmdline_parser (int argc, char * const *argv, struct gengetopt_args_info *args_i
         { NULL,	0, NULL, 0 }
       };
 
-      c = getopt_long (argc, argv, "hVdc:l:r:a:i:m:q:u:p:", long_options, &option_index);
+      c = getopt_long (argc, argv, "hVdc:l:r:a:i:m:q:u:p:n:", long_options, &option_index);
 
       if (c == -1) break;	/* Exit from `while (1)' loop.  */
 
@@ -311,6 +323,17 @@ cmdline_parser (int argc, char * const *argv, struct gengetopt_args_info *args_i
           args_info->pwd_arg = strdup (optarg);
           break;
 
+        case 'n':	/* Network address for local interface.  */
+          if (args_info->net_given)
+            {
+              fprintf (stderr, "%s: `--net' (`-n') option given more than once\n", PACKAGE);
+              clear_args ();
+              exit (EXIT_FAILURE);
+            }
+          args_info->net_given = 1;
+          args_info->net_arg = strdup (optarg);
+          break;
+
 
         case 0:	/* Long option with no short option */
           /* Filename of process id file.  */
@@ -378,6 +401,32 @@ cmdline_parser (int argc, char * const *argv, struct gengetopt_args_info *args_i
             args_info->timelimit_arg = strtol (optarg,&stop_char,0);
             break;
           }
+          /* GTP version to use.  */
+          else if (strcmp (long_options[option_index].name, "gtpversion") == 0)
+          {
+            if (args_info->gtpversion_given)
+              {
+                fprintf (stderr, "%s: `--gtpversion' option given more than once\n", PACKAGE);
+                clear_args ();
+                exit (EXIT_FAILURE);
+              }
+            args_info->gtpversion_given = 1;
+            args_info->gtpversion_arg = strtol (optarg,&stop_char,0);
+            break;
+          }
+          /* NSAPI.  */
+          else if (strcmp (long_options[option_index].name, "nsapi") == 0)
+          {
+            if (args_info->nsapi_given)
+              {
+                fprintf (stderr, "%s: `--nsapi' option given more than once\n", PACKAGE);
+                clear_args ();
+                exit (EXIT_FAILURE);
+              }
+            args_info->nsapi_given = 1;
+            args_info->nsapi_arg = strtol (optarg,&stop_char,0);
+            break;
+          }
           /* Create local network interface.  */
           else if (strcmp (long_options[option_index].name, "createif") == 0)
           {
@@ -705,6 +754,22 @@ cmdline_parser_configfile (char * const filename, struct gengetopt_args_info *ar
                 }
               continue;
             }
+          if (!strcmp(fopt, "gtpversion"))
+            {
+              if (override || !args_info->gtpversion_given)
+                {
+                  args_info->gtpversion_given = 1;
+                  if (fnum == 2)
+                    args_info->gtpversion_arg = strtol (farg,&stop_char,0);
+                  else
+                    {
+                      fprintf (stderr, "%s:%d: required <option_name> <option_val>\n",
+                               filename, line_num);
+                      exit (EXIT_FAILURE);
+                    }
+                }
+              continue;
+            }
           if (!strcmp(fopt, "apn"))
             {
               if (override || !args_info->apn_given)
@@ -737,6 +802,22 @@ cmdline_parser_configfile (char * const filename, struct gengetopt_args_info *ar
                 }
               continue;
             }
+          if (!strcmp(fopt, "nsapi"))
+            {
+              if (override || !args_info->nsapi_given)
+                {
+                  args_info->nsapi_given = 1;
+                  if (fnum == 2)
+                    args_info->nsapi_arg = strtol (farg,&stop_char,0);
+                  else
+                    {
+                      fprintf (stderr, "%s:%d: required <option_name> <option_val>\n",
+                               filename, line_num);
+                      exit (EXIT_FAILURE);
+                    }
+                }
+              continue;
+            }
           if (!strcmp(fopt, "msisdn"))
             {
               if (override || !args_info->msisdn_given)
@@ -810,6 +891,22 @@ cmdline_parser_configfile (char * const filename, struct gengetopt_args_info *ar
                 }
               continue;
             }
+          if (!strcmp(fopt, "net"))
+            {
+              if (override || !args_info->net_given)
+                {
+                  args_info->net_given = 1;
+                  if (fnum == 2)
+                    args_info->net_arg = strdup (farg);
+                  else
+                    {
+                      fprintf (stderr, "%s:%d: required <option_name> <option_val>\n",
+                               filename, line_num);
+                      exit (EXIT_FAILURE);
+                    }
+                }
+              continue;
+            }
           if (!strcmp(fopt, "defaultroute"))
             {
               if (override || !args_info->defaultroute_given)
diff --git a/sgsnemu/cmdline.ggo b/sgsnemu/cmdline.ggo
index 2c4f447..1d0ea75 100644
--- a/sgsnemu/cmdline.ggo
+++ b/sgsnemu/cmdline.ggo
@@ -28,14 +28,17 @@ option  "remote"       r "Remote host"                    string no
 option  "contexts"     - "Number of contexts"             int    default="1" no
 option  "timelimit"    - "Exit after timelimit seconds"   int default="0" no
 
+option  "gtpversion"   - "GTP version to use"             int    default="1" no
 option  "apn"          a "Access point name"              string default="internet" no
 option  "imsi"         i "IMSI"                           string default="240010123456789" no
+option  "nsapi"        - "NSAPI"                          int default="0" no
 option  "msisdn"       m "Mobile Station ISDN number"     string default="46702123456" no
 option  "qos"          q "Requested quality of service"   int    default="0x0b921f" no
 option  "uid"          u "Login user ID"                  string default="mig" no
 option  "pwd"          p "Login password"                 string default="hemmelig" no
 
 option  "createif"     - "Create local network interface" flag   off
+option  "net"          n "Network address for local interface" string no
 option  "defaultroute" - "Create default route"           flag   off
 option  "ipup"         - "Script to run after link-up"    string no
 option  "ipdown"       - "Script to run after link-down"  string no
diff --git a/sgsnemu/cmdline.h b/sgsnemu/cmdline.h
index 636dcc3..2d9b7e5 100644
--- a/sgsnemu/cmdline.h
+++ b/sgsnemu/cmdline.h
@@ -29,13 +29,16 @@ struct gengetopt_args_info
   char * remote_arg;	/* Remote host.  */
   int contexts_arg;	/* Number of contexts (default='1').  */
   int timelimit_arg;	/* Exit after timelimit seconds (default='0').  */
+  int gtpversion_arg;	/* GTP version to use (default='1').  */
   char * apn_arg;	/* Access point name (default='internet').  */
   char * imsi_arg;	/* IMSI (default='240010123456789').  */
+  int nsapi_arg;	/* NSAPI (default='1').  */
   char * msisdn_arg;	/* Mobile Station ISDN number (default='46702123456').  */
   int qos_arg;	/* Requested quality of service (default='0x0b921f').  */
   char * uid_arg;	/* Login user ID (default='mig').  */
   char * pwd_arg;	/* Login password (default='hemmelig').  */
   int createif_flag;	/* Create local network interface (default=off).  */
+  char * net_arg;	/* Network address for local interface.  */
   int defaultroute_flag;	/* Create default route (default=off).  */
   char * ipup_arg;	/* Script to run after link-up.  */
   char * ipdown_arg;	/* Script to run after link-down.  */
@@ -56,13 +59,16 @@ struct gengetopt_args_info
   int remote_given ;	/* Whether remote was given.  */
   int contexts_given ;	/* Whether contexts was given.  */
   int timelimit_given ;	/* Whether timelimit was given.  */
+  int gtpversion_given ;	/* Whether gtpversion was given.  */
   int apn_given ;	/* Whether apn was given.  */
   int imsi_given ;	/* Whether imsi was given.  */
+  int nsapi_given ;	/* Whether nsapi was given.  */
   int msisdn_given ;	/* Whether msisdn was given.  */
   int qos_given ;	/* Whether qos was given.  */
   int uid_given ;	/* Whether uid was given.  */
   int pwd_given ;	/* Whether pwd was given.  */
   int createif_given ;	/* Whether createif was given.  */
+  int net_given ;	/* Whether net was given.  */
   int defaultroute_given ;	/* Whether defaultroute was given.  */
   int ipup_given ;	/* Whether ipup was given.  */
   int ipdown_given ;	/* Whether ipdown was given.  */
diff --git a/sgsnemu/sgsnemu.c b/sgsnemu/sgsnemu.c
index e636f1b..c63f634 100644
--- a/sgsnemu/sgsnemu.c
+++ b/sgsnemu/sgsnemu.c
@@ -59,7 +59,7 @@
 #include "cmdline.h"
 
 #define IPADDRLEN 256      /* Character length of addresses */ 
-#define MAXCONTEXTS 16     /* Max number of allowed contexts */ 
+#define MAXCONTEXTS 1024  /* Max number of allowed contexts */ 
 
 /* HASH tables for IP address allocation */
 struct iphash_t {
@@ -89,6 +89,7 @@ int echoversion = 1;            /* First try this version */
 struct {
   int debug;                      /* Print debug messages */
   int createif;                   /* Create local network interface */
+  struct in_addr net, mask;       /* Network interface       */
   char *ipup, *ipdown;            /* Filename of scripts */
   int defaultroute;               /* Set up default route */
   struct in_addr pinghost;        /* Remote ping host    */
@@ -103,6 +104,8 @@ struct {
   int timelimit;                  /* Number of seconds to be connected */
   char *statedir;
   uint64_t imsi;
+  uint8_t nsapi;
+  int gtpversion;
   struct ul255_t pco;
   struct ul255_t qos;
   struct ul255_t apn;
@@ -389,8 +392,18 @@ int process_options(int argc, char **argv) {
   options.imsi |= ((uint64_t) (args_info.imsi_arg[13]-48)) << 52;
   options.imsi |= ((uint64_t) (args_info.imsi_arg[14]-48)) << 56;
 
-    printf("IMSI is:               %s (%#08llx)\n", 
-	   args_info.imsi_arg, options.imsi);
+  printf("IMSI is:               %s (%#08llx)\n", 
+	 args_info.imsi_arg, options.imsi);
+  
+  
+  /* nsapi                                                           */
+  if ((args_info.nsapi_arg > 15) || 
+      (args_info.nsapi_arg < 0)) {
+    printf("Invalid NSAPI\n");
+    return -1;
+  }
+  options.nsapi = args_info.nsapi_arg;
+  printf("Using NSAPI:           %d\n", args_info.nsapi_arg);
 
 
   /* qos                                                             */
@@ -409,6 +422,16 @@ int process_options(int argc, char **argv) {
   /* Timelimit                                                       */
   options.timelimit = args_info.timelimit_arg;
   
+  /* gtpversion                                                      */
+  if ((args_info.gtpversion_arg > 1) || 
+      (args_info.gtpversion_arg < 0)) {
+    printf("Invalid GTP version\n");
+    return -1;
+  }
+  options.gtpversion = args_info.gtpversion_arg;
+  printf("Using GTP version:     %d\n", args_info.gtpversion_arg);
+
+
   /* apn                                                             */
   if (strlen(args_info.apn_arg) > (sizeof(options.apn.v)-1)) {
     printf("Invalid APN\n");
@@ -464,6 +487,20 @@ int process_options(int argc, char **argv) {
   /* createif */
   options.createif = args_info.createif_flag;
 
+  /* net                                                          */
+  /* Store net as in_addr net and mask                            */
+  if (args_info.net_arg) {
+    if(ippool_aton(&options.net, &options.mask, args_info.net_arg, 0)) {
+      sys_err(LOG_ERR, __FILE__, __LINE__, 0,
+	      "Invalid network address: %s!", args_info.net_arg);
+      exit(1);
+    }
+  }
+  else {
+    options.net.s_addr = 0;
+    options.mask.s_addr = 0;
+  }
+
   /* ipup */
   options.ipup = args_info.ipup_arg;
 
@@ -544,6 +581,81 @@ char * print_icmptype(int t) {
   return(ttab[t]);
 }
 
+int msisdn_add(struct ul16_t *src, struct ul16_t *dst, int add) {
+  int n;
+  uint64_t i64 = 0;
+  uint8_t msa[sizeof(i64) * 3]; /* Allocate 3 digits per octet (0..255) */
+  int msalen = 0;
+
+  /* Convert to uint64_t from ul16_t format (most significant digit first) */
+  /* ul16_t format always starts with 0x91 to indicate international format */
+  /* In ul16_t format 0x0f/0xf0 indicates that digit is not used */
+  for (n=0; n< src->l; n++) {
+    if ((src->v[n] & 0x0f) != 0x0f) {
+      i64 *= 10;
+      i64 += src->v[n] & 0x0f;
+    }
+    if ((src->v[n] & 0xf0) != 0xf0) {
+      i64 *= 10;
+      i64 += (src->v[n] & 0xf0) >> 4;
+    }
+  }
+
+  i64 += add;
+
+  /* Generate array with least significant digit in first octet */
+  while (i64) {
+    msa[msalen++] = i64 % 10;
+    i64 = i64 / 10;
+  }
+
+  /* Convert back to ul16_t format */
+  for(n=0; n<msalen; n++) {
+    if ((n%2) == 0) {
+      dst->v[((int)n/2)] = msa[msalen-n-1] + 0xf0;
+      dst->l += 1;
+    }
+    else {
+      dst->v[((int)n/2)] = (dst->v[((int)n/2)] & 0x0f) + 
+	msa[msalen-n-1] * 16;
+    }
+  }
+
+  return 0;
+
+}
+
+int imsi_add(uint64_t src, uint64_t *dst, int add) {
+  /* TODO: big endian / small endian ??? */
+  uint64_t i64 = 0;
+
+  /* Convert from uint64_t bcd to uint64_t integer format */
+  /* The resulting integer format is multiplied by 10 */
+  while (src) {
+    if ((src & 0x0f) != 0x0f) {
+      i64 *= 10;
+      i64 += (src & 0x0f);
+    }
+    if ((src & 0xf0) != 0xf0) {
+      i64 *= 10;
+      i64 += (src & 0xf0) >> 4;
+    }
+    src = src >> 8;
+  }
+
+  i64 += add * 10; 
+
+  *dst = 0;
+  while (i64) {
+    *dst = *dst << 4;
+    *dst += (i64 % 10);
+    i64 = i64 / 10;
+  }
+
+  return 0;
+
+}
+
 /* Calculate time left until we have to send off next ping packet */
 int ping_timeout(struct timeval *tp) {
   struct timezone tz;
@@ -786,7 +898,7 @@ int create_pdp_conf(struct pdp_t *pdp, void *cbp, int cause) {
     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);
+      gtp_create_context_req(gsn, iph->pdp, iph);
       return 0;
     }
     else {
@@ -816,7 +928,7 @@ int create_pdp_conf(struct pdp_t *pdp, void *cbp, int cause) {
   printf("Received create PDP context response. IP address: %s\n", 
 	 inet_ntoa(addr));
 
-  if (options.createif) {
+  if ((options.createif) && (!options.net.s_addr)) {
     struct in_addr m;
     inet_aton("255.255.255.255", &m);
     /* printf("Setting up interface and routing\n");*/
@@ -935,6 +1047,18 @@ int main(int argc, char **argv)
     if (tun->fd > maxfd) maxfd = tun->fd;
   }
 
+  if ((options.createif) && (options.net.s_addr)) {
+    /* printf("Setting up interface and routing\n");*/
+    tun_addaddr(tun, &options.net,  &options.net, &options.mask);
+    if (options.defaultroute) {
+      struct in_addr rm;
+      rm.s_addr = 0;
+      tun_addroute(tun, &rm,  &options.net, &rm);
+    }
+    if (options.ipup) tun_runscript(tun, options.ipup);
+  }
+
+
   /* Initialise hash tables */
   memset(&iphash, 0, sizeof(iphash));  
   memset(&iparr, 0, sizeof(iparr));  
@@ -943,16 +1067,20 @@ int main(int argc, char **argv)
 
   /* See if anybody is there */
   printf("Sending off echo request\n");
+  echoversion = options.gtpversion;
   gtp_echo_req(gsn, echoversion, NULL, &options.remote); /* Is remote alive? */
 
   for(n=0; n<options.contexts; n++) {
+    uint64_t myimsi;
     printf("Setting up PDP context #%d\n", n);
     iparr[n].inuse = 1; /* TODO */
 
+    imsi_add(options.imsi, &myimsi, n);
+
     /* Allocated here. */
     /* If create context failes we have to deallocate ourselves. */
-    /* Otherwise it is deallocated gy gtplib */
-    pdp_newpdp(&pdp, options.imsi, n, NULL); 
+    /* Otherwise it is deallocated by gtplib */
+    pdp_newpdp(&pdp, myimsi, options.nsapi, NULL); 
 
     pdp->peer = &iparr[n];
     pdp->ipif = tun; /* TODO */
@@ -992,8 +1120,7 @@ int main(int argc, char **argv)
       exit(1);
     }
     else {
-      pdp->msisdn.l = options.msisdn.l;
-      memcpy(pdp->msisdn.v, options.msisdn.v, options.msisdn.l);
+      msisdn_add(&options.msisdn, &pdp->msisdn, n);
     }
     
     ipv42eua(&pdp->eua, NULL); /* Request dynamic IP address */
@@ -1007,11 +1134,14 @@ int main(int argc, char **argv)
       memcpy(pdp->pco_req.v, options.pco.v, options.pco.l);
     }
     
-    pdp->version = 1; /* First try with version 1 */
+    pdp->version = options.gtpversion;
+
+    pdp->hisaddr0 = options.remote;
+    pdp->hisaddr1 = options.remote;
 
     /* Create context */
     /* We send this of once. Retransmissions are handled by gtplib */
-    gtp_create_context_req(gsn, pdp, &iparr[n], &options.remote);
+    gtp_create_context_req(gsn, pdp, &iparr[n]);
   }    
 
   state = 1;  /* Enter wait_connection state */