sgsnemu.c

NGN－3G核心部件SGSN的代码实现

  /* apn                                                             */
  if (strlen(args_info.apn_arg) > (sizeof(options.apn.v)-1)) {
    printf("Invalid APN\n");
    return -1;
  }
  options.apn.l = strlen(args_info.apn_arg) + 1;
  options.apn.v[0] = (char) strlen(args_info.apn_arg);
  strncpy(&options.apn.v[1], args_info.apn_arg, sizeof(options.apn.v)-1);
  printf("Using APN:             %s\n", args_info.apn_arg);


  /* selmode */
  options.selmode = args_info.selmode_arg;
  printf("Using selection mode:  %d\n", args_info.selmode_arg);

  
  /* msisdn                                                          */
  if (strlen(args_info.msisdn_arg)>(sizeof(options.msisdn.v)-1)) {
    printf("Invalid MSISDN\n");
    return -1;
  }
  options.msisdn.l = 1;
  options.msisdn.v[0] = 0x91; /* International format */
  for(n=0; n<strlen(args_info.msisdn_arg); n++) {
    if ((n%2) == 0) {
      options.msisdn.v[((int)n/2)+1] = args_info.msisdn_arg[n] - 48 + 0xf0;
      options.msisdn.l += 1;
    }
    else {
      options.msisdn.v[((int)n/2)+1] = 
	(options.msisdn.v[((int)n/2)+1] & 0x0f) +
	(args_info.msisdn_arg[n] - 48) * 16;
    }
  }
  printf("Using MSISDN:          %s\n", args_info.msisdn_arg);

  /* UID and PWD */
  /* Might need to also insert stuff like DNS etc. */
  if ((strlen(args_info.uid_arg) + strlen(args_info.pwd_arg) + 10)>
      (sizeof(options.pco.v)-1)) {
    printf("invalid UID and PWD\n");
    return -1;
  }
  options.pco.l = strlen(args_info.uid_arg) + strlen(args_info.pwd_arg) + 10;
  options.pco.v[0] = 0x80; /* PPP */
  options.pco.v[1] = 0xc0; /* PAP */
  options.pco.v[2] = 0x23; 
  options.pco.v[3] = strlen(args_info.uid_arg) + strlen(args_info.pwd_arg) + 6;
  options.pco.v[4] = 0x01; /* Authenticate request */
  options.pco.v[5] = 0x01;
  options.pco.v[6] = 0x00; /* MSB of length */
  options.pco.v[7] = strlen(args_info.uid_arg) + strlen(args_info.pwd_arg) + 6;
  options.pco.v[8] = strlen(args_info.uid_arg);
  memcpy(&options.pco.v[9], args_info.uid_arg, strlen(args_info.uid_arg));
  options.pco.v[9+strlen(args_info.uid_arg)] = strlen(args_info.pwd_arg);
  memcpy(&options.pco.v[10+strlen(args_info.uid_arg)], 
	 args_info.pwd_arg, strlen(args_info.pwd_arg));
  
  /* 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);
    }

#if defined (__sun__)
    options.netaddr.s_addr = htonl(ntohl(options.net.s_addr) + 1);
    options.destaddr.s_addr = htonl(ntohl(options.net.s_addr) + 1);
#else
    options.netaddr.s_addr = options.net.s_addr;
    options.destaddr.s_addr = options.net.s_addr;
#endif

  }
  else {
    options.net.s_addr = 0;
    options.mask.s_addr = 0;
    options.netaddr.s_addr = 0;
    options.destaddr.s_addr = 0;
  }

  /* ipup */
  options.ipup = args_info.ipup_arg;

  /* ipdown */
  options.ipdown = args_info.ipdown_arg;

  /* statedir */
  options.statedir = args_info.statedir_arg;

  /* defaultroute */
  options.defaultroute = args_info.defaultroute_flag;


  /* pinghost                                                     */
  /* Store ping host as in_addr                                   */
  if (args_info.pinghost_arg) {
    if (!(host = gethostbyname(args_info.pinghost_arg))) {
      sys_err(LOG_ERR, __FILE__, __LINE__, 0,
	      "Invalid ping host: %s!", args_info.pinghost_arg);
      return -1;
    }
    else {
      memcpy(&options.pinghost.s_addr, host->h_addr, host->h_length);
      printf("Using ping host:       %s (%s)\n", 
	     args_info.pinghost_arg, inet_ntoa(options.pinghost));
    }
  }

  /* Other ping parameters                                        */
  options.pingrate = args_info.pingrate_arg;
  options.pingsize = args_info.pingsize_arg;
  options.pingcount = args_info.pingcount_arg;
  options.pingquiet = args_info.pingquiet_flag;

  return 0;

}


int encaps_printf(struct pdp_t *pdp, void *pack, unsigned len) {
  int i;
  printf("The packet looks like this:\n");
  for( i=0; i<len; i++) {
    printf("%02x ", (unsigned char)*(char *)(pack+i));
    if (!((i+1)%16)) printf("\n");
  };
  printf("\n");
  return 0;
}

char * print_ipprot(int t) {
  switch (t) {
  case  1: return "ICMP";
  case  6: return "TCP";
  case 17: return "UDP";
  default: return "Unknown";
  };
}


char * print_icmptype(int t) {
  static char *ttab[] = {
    "Echo Reply",
    "ICMP 1",
    "ICMP 2",
    "Dest Unreachable",
    "Source Quench",
    "Redirect",
    "ICMP 6",
    "ICMP 7",
    "Echo",
    "ICMP 9",
    "ICMP 10",
    "Time Exceeded",
    "Parameter Problem",
    "Timestamp",
    "Timestamp Reply",
    "Info Request",
    "Info Reply"
  };
  if( t < 0 || t > 16 )
    return("OUT-OF-RANGE");  
  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;
  }

  *dst |= 0xf000000000000000ull;

  return 0;

}

/* Calculate time left until we have to send off next ping packet */
int ping_timeout(struct timeval *tp) {
  struct timezone tz;
  struct timeval tv;
  int diff;
  if ((options.pinghost.s_addr) && (2 == state) && 
      ((pingseq < options.pingcount) || (options.pingcount == 0))) {
    gettimeofday(&tv, &tz);
    diff = 1000000 / options.pingrate * pingseq -
      1000000 * (tv.tv_sec - firstping.tv_sec) -
      (tv.tv_usec - firstping.tv_usec); /* Microseconds safe up to 500 sec */
    tp->tv_sec = 0;
    if (diff > 0) 
      tp->tv_usec = diff;
    else {
      /* For some reason we get packet loss if set to zero */
      tp->tv_usec = 100000 / options.pingrate; /* 10 times pingrate */
	 tp->tv_usec = 0; 
    }
  }
  return 0;
}

/* Print out statistics when at the end of ping sequence */
int ping_finish()
{
  struct timezone tz;
  struct timeval tv;
  int elapsed;
  gettimeofday(&tv, &tz);
  elapsed = 1000000 * (tv.tv_sec - firstping.tv_sec) + 
    (tv.tv_usec - firstping.tv_usec); /* Microseconds */
  printf("\n");
  printf("\n----%s PING Statistics----\n", inet_ntoa(options.pinghost));
  printf("%d packets transmitted in %.3f seconds, ", ntransmitted,
	 elapsed / 1000000.0);
  printf("%d packets received, ", nreceived );
  if (ntransmitted) {
    if( nreceived > ntransmitted)
      printf("-- somebody's printing up packets!");
    else
      printf("%d%% packet loss", 
	     (int) (((ntransmitted-nreceived)*100) /
		    ntransmitted));
  }
  printf("\n");
  if (options.debug) printf("%d packets received in total\n", ntreceived );
  if (nreceived  && tsum)
    printf("round-trip (ms)  min/avg/max = %.3f/%.3f/%.3f\n\n",
	   tmin/1000.0,
	   tsum/1000.0/nreceived,
	   tmax/1000.0 );
  printf("%d packets transmitted \n", ntreceived );

  ntransmitted = 0;
  return 0;
}

/* Handle a received ping packet. Print out line and update statistics. */
int encaps_ping(struct pdp_t *pdp, void *pack, unsigned len) {
  struct timezone tz;
  struct timeval tv;
  struct timeval *tp;
  struct ip_ping *pingpack = pack;
  struct in_addr src;
  int triptime;

  src.s_addr = pingpack->src;

  gettimeofday(&tv, &tz);
  if (options.debug) printf("%d.%6d ", (int) tv.tv_sec, (int) tv.tv_usec);

  if (len < CREATEPING_IP + CREATEPING_ICMP) {
    printf("packet too short (%d bytes) from %s\n", len,
	   inet_ntoa(src));
    return 0;
  }

  ntreceived++;
  if (pingpack->protocol != 1) {
    if (!options.pingquiet) printf("%d bytes from %s: ip_protocol=%d (%s)\n",
	   len, inet_ntoa(src), pingpack->protocol, 
	   print_ipprot(pingpack->protocol));
    return 0;
  }

  if (pingpack->type != 0) {
    if (!options.pingquiet) printf("%d bytes from %s: icmp_type=%d (%s) icmp_code=%d\n",
	   len, inet_ntoa(src), pingpack->type, 
	   print_icmptype(pingpack->type), pingpack->code);
    return 0;
  }

  nreceived++;
  if (!options.pingquiet) printf("%d bytes from %s: icmp_seq=%d", len,
	 inet_ntoa(src), ntohs(pingpack->seq));

  if (len >= sizeof(struct timeval) + CREATEPING_IP + CREATEPING_ICMP) {
    gettimeofday(&tv, &tz);
    tp = (struct timeval *) pingpack->data;
    if( (tv.tv_usec -= tp->tv_usec) < 0 )   {
      tv.tv_sec--;
      tv.tv_usec += 1000000;
    }
    tv.tv_sec -= tp->tv_sec;

    triptime = tv.tv_sec*1000000+(tv.tv_usec);
    tsum += triptime;
    if( triptime < tmin )
      tmin = triptime;
    if( triptime > tmax )
      tmax = triptime;

    if (!options.pingquiet) printf(" time=%.3f ms\n", triptime/1000.0);

  } 
  else
    if (!options.pingquiet) printf("\n");
  return 0;
}

/* Create a new ping packet and send it off to peer. */
int create_ping(void *gsn, struct pdp_t *pdp,
		struct in_addr *dst, int seq, int datasize) {

  struct ip_ping pack;
  uint16_t *p = (uint16_t *) &pack;
  uint8_t  *p8 = (uint8_t *) &pack;
  struct in_addr src;
  int n;
  long int sum = 0;
  int count = 0;

  struct timezone tz;
  struct timeval *tp = (struct timeval *) &p8[CREATEPING_IP + CREATEPING_ICMP];

  if (datasize > CREATEPING_MAX) {
    sys_err(LOG_ERR, __FILE__, __LINE__, 0,
	    "Ping size to large: %d!", datasize);
    return -1;
  }

  memcpy(&src, &(pdp->eua.v[2]), 4); /* Copy a 4 byte address */

  pack.ipver  = 0x45;
  pack.tos    = 0x00;
  pack.length = htons(CREATEPING_IP + CREATEPING_ICMP + datasize);
  pack.fragid = 0x0000;
  pack.offset = 0x0040;
  pack.ttl    = 0x40;
  pack.protocol = 0x01;
  pack.ipcheck = 0x0000;
  pack.src = src.s_addr;
  pack.dst = dst->s_addr;
  pack.type = 0x08;
  pack.code = 0x00;
  pack.checksum = 0x0000;
  pack.ident = 0x0000;
  pack.seq = htons(seq);

  /* Generate ICMP payload */
  p8 = (uint8_t *) &pack + CREATEPING_IP + CREATEPING_ICMP;
  for (n=0; n<(datasize); n++) p8[n] = n;

  if (datasize >= sizeof(struct timeval)) 
    gettimeofday(tp, &tz);

  /* Calculate IP header checksum */
  p = (uint16_t *) &pack;
  count = CREATEPING_IP;
  sum = 0;
  while (count>1) {
    sum += *p++;
    count -= 2;
  }
  while (sum>>16) 
    sum = (sum & 0xffff) + (sum >> 16);
  pack.ipcheck = ~sum;


  /* Calculate ICMP checksum */
  count = CREATEPING_ICMP + datasize; /* Length of ICMP message */
  sum = 0;
  p = (uint16_t *) &pack;
  p += CREATEPING_IP / 2;
  while (count>1) {
    sum += *p++;
    count -= 2;