ldltest.c

7号信令功能代码,为开源代码

	    int             i;

	    printf("    dsap: ");
	    for (i = 0; i < addr_len + sap_len; i++)
		  printf("%02x ", dst_dlsap[i] & 0xff);

	    printf("  ssap: ");
	    for (i = 0; i < addr_len + sap_len; i++)
		  printf("%02x ", src_dlsap[i] & 0xff);
	    printf("\n");
      }

      return 0;
}

int check_ip_packet(unsigned char *data, int datalen)
{

      struct iphdr   *hdr = (struct iphdr *) data;

      if (datalen < sizeof(*hdr)) {
	    if (verbose)
		  fprintf(stderr, "check_ip_packet: Too short for IP header.\n");
	    return -1;
      }
      if (hdr->version != 4) {
	    if (verbose)
		  fprintf(stderr, "check_ip_packet: Not IP version 4.\n");
	    return -1;
      }
      if (hdr->ihl != 5) {
	    if (verbose)
		  fprintf(stderr, "check_ip_packet: Cannot handle IP options.\n");
	    return -1;
      }
      if ((hdr->frag_off & ~0x40) != 0) {
	    if (verbose)
		  fprintf(stderr, "check_ip_packet: Cannot handle fragmented packets.\n");
	    return -1;
      }
      if (in_cksum(hdr, sizeof(*hdr)) != 0) {
	    if (verbose)
		  fprintf(stderr, "check_ip_packet: Bad IP header checksum\n");
	    return -1;
      }
      if (ntohs(hdr->tot_len) > datalen) {
	    if (verbose)
		  fprintf(stderr, "check_ip_packet: Shorter than IP header says.\n");
	    return -1;
      }
      if (hdr->daddr != my_ip_addr.s_addr) {
	    if (verbose > 3) {
		  struct in_addr  ip_addr;

		  printf("check_ip_packet: Not for my address (%s)",
			 inet_ntoa(my_ip_addr));
		  memcpy(&ip_addr, &hdr->daddr, sizeof(ip_addr));
		  printf(", but for %s\n", inet_ntoa(ip_addr));
	    }
	    return -1;
      }

      return 0;
}

int do_rcv_unitdata(int fd, unsigned char *data, int *datalen, unsigned char *src_dlsap,
		    unsigned char *dst_dlsap)
{
      int             dlen;
      unsigned short  pkt_type;

      for (;;) {
	    dlen = *datalen;
	    if (_do_rcv_unitdata(fd, data, &dlen, src_dlsap, dst_dlsap) < 0)
		  return -1;

	    if (mac_type == DL_ETHER)
		  pkt_type = *(unsigned short *) (dst_dlsap + addr_len);
	    else
		  pkt_type = 0x0000;

	    if (pkt_type == ETH_P_ARP) {
		  if (verbose > 2)
			printf("do_rcv_unitdata(): Got ARP packet.\n");
		  process_incoming_arp(fd, data, dlen, src_dlsap);

		  continue;
	    }

	    if (pkt_type == ETH_P_IP) {
		  if (verbose > 2)
			printf("do_rcv_unitdata(): Got IP packet.\n");
		  if (memcmp(my_addr, dlsap_addr(dst_dlsap), addr_len)) {
			if (verbose > 2)
			      printf
				  ("do_rcv_unitdata(): IP packet not for my MAC address.\n");
			continue;
		  }
		  if (check_ip_packet(data, dlen) < 0)
			continue;
		  if (((struct iphdr *) data)->protocol == 1 /* ICMP */  &&
		      ((struct ping_pkt *) data)->icmp_hdr.type == ICMP_ECHO) {
			process_incoming_echo_request(fd, data, dlen, src_dlsap);
			continue;
		  }
	    }
	    else if (ntohs(pkt_type) < 1536) {
		  if (verbose > 2)
			printf("do_rcv_unitdata(): Got LLC frame.\n");
		  if (memcmp(my_addr, dlsap_addr(dst_dlsap), addr_len)) {
			if (verbose > 2)
			      printf
				  ("do_rcv_unitdata(): LLC frame not for my MAC address.\n");
			continue;
		  }
		  if (process_incoming_llc_request(fd, data, dlen, src_dlsap, dst_dlsap)
		      <= 0)
			continue;	/* keep going if cmnd rcvd */
		  /*
		   * fall thru and return if resp received 
		   */
	    }
	    *datalen = dlen;
	    return 0;
      }
}

jmp_buf         timeout_jmp;

static void arp_timeout(int arg)
{
      longjmp(timeout_jmp, 1);
}

int do_arp(int fd, struct in_addr ip, unsigned char *target_addr)
{
      /*
       *  Sending out an ICMP PING packet using the DLPI directly
       *  is a bit hard as we have to know the datalink address of
       *  the node (host or router) that we should use. This address
       *  is normally found using the Address Resolution Protocol,
       *  but DLPI gives us no ARP service.
       *
       *  This means that we have to do the ARP protocol manually.
       *
       *  This will not work across routers (unless the router does
       *  proxy ARP for our target).
       *  Anybody that wants to fix this: Feel free to add /proc/net/route
       *  lookup. (Or better: Do an IP Network Provider Interface).
       *
       */
      char            buf[1024];

      if (verbose > 2)
	    printf("do_arp: Trying to do ARP lookup.\n");

#if 0
      /*
       *  We have no arp on the loopback device
       */
      if (!strcmp((char *) inet_ntoa(ip), "127.0.0.1")) {
#if 0
	    my_ip_addr = ip;
#endif
	    memset(target_addr, 0, ETH_ALEN);
	    build_dlsap(target_dlsap, target_addr, dlsap_sap(my_dlsap));
	    if (verbose > 2)
		  printf("do_arp: ARP lookup on loopback.\n");
	    return 0;
      }
#endif

      if (send_arp_request(fd) < 0) {
	    if (verbose)
		  fprintf(stderr, "do_arp: send_arp_request() failed\n");
	    return -1;
      }

      got_target_addr = 0;
      if (setjmp(timeout_jmp) != 0) {
	    alarm(0);
	    signal(SIGALRM, SIG_IGN);
	    if (!got_target_addr) {
		  if (verbose)
			fprintf(stderr, "Timed out while waiting for ARP reply\n");
		  return -1;
	    }
	    build_dlsap(target_dlsap, target_addr, dlsap_sap(my_dlsap));
	    return 0;
      }
      signal(SIGALRM, arp_timeout);
      alarm(2000);
      for (;;) {		/* Loop broken by signal */
	    int             buflen;
	    unsigned char   src_dlsap[MAX_DL_ADDR_LEN + MAX_DL_SAP_LEN];
	    unsigned char   dst_dlsap[MAX_DL_ADDR_LEN + MAX_DL_SAP_LEN];

	    buflen = 1000;
	    if (do_rcv_unitdata(fd, buf, &buflen, src_dlsap, dst_dlsap) < 0)
		  return -1;
      }
}

/**************************************************************/
/*
 */
/*
 * The ping test routines 
 */
/*
 */
/**************************************************************/

int send_ping(int fd, __u16 id, __u16 sequence)
{
      struct ping_pkt pkt;

      if (verbose > 1) {
	    printf("send_ping: sending ping.\n");
	    printf("send_ping: Target: IP=%s\n", inet_ntoa(target_ip_addr));
      }

      pkt.ip_hdr.version = 4;
      pkt.ip_hdr.ihl = 5;
      pkt.ip_hdr.tos = 0;
      pkt.ip_hdr.tot_len = htons(sizeof(pkt));
      pkt.ip_hdr.id = 1;
      pkt.ip_hdr.frag_off = 0;
      pkt.ip_hdr.ttl = 4;	/* We don't want too much network trouble. ;-) */
      pkt.ip_hdr.protocol = 1;	/* ICMP */
      pkt.ip_hdr.check = 0;
      pkt.ip_hdr.daddr = (__u32) target_ip_addr.s_addr;
      pkt.ip_hdr.saddr = (__u32) my_ip_addr.s_addr;
      pkt.ip_hdr.check = in_cksum(&pkt.ip_hdr, sizeof(struct iphdr));

      pkt.icmp_hdr.type = ICMP_ECHO;
      pkt.icmp_hdr.code = 0;
      pkt.icmp_hdr.checksum = 0;
      pkt.icmp_hdr.un.echo.id = id;
      pkt.icmp_hdr.un.echo.sequence = sequence;

      if (gettimeofday(&pkt.tv, NULL) < 0) {
	    perror("send_ping: gettimeofday()");
	    return -1;
      }
      memset(pkt.filler, 0, sizeof(pkt.filler));
      pkt.icmp_hdr.checksum = in_cksum(&pkt.icmp_hdr, sizeof(pkt) - sizeof(pkt.ip_hdr));

      if (do_send_unitdata(fd, (char *) &pkt, sizeof(pkt), target_dlsap) < 0) {
	    if (verbose)
		  fprintf(stderr, "send_ping: do_send_unitdata() failed.\n");
	    return -1;
      }

      if (verbose > 1)
	    printf("send_ping: sent ping.\n");
      return 0;
}

int rcv_ping(int fd, __u16 id, __u16 * sequence)
{
      int             buflen;
      struct {
	    struct ping_pkt ip_pkt __attribute__ ((packed));
	    unsigned char   extra[1024];
      } pkt;
      unsigned char  *saddr,
                     *ssap;
      unsigned char  *daddr,
                     *dsap;
      char            strs[64],
                      strd[64];
      unsigned char   s_dlsap[MAX_DL_ADDR_LEN + MAX_DL_SAP_LEN];
      unsigned char   d_dlsap[MAX_DL_ADDR_LEN + MAX_DL_SAP_LEN];
      struct timeval  tv;
      double          rtt;

      if (verbose > 3)
	    printf("--\nrcv_ping: Waiting for data...\n");

      buflen = sizeof(pkt);
      if (do_rcv_unitdata(fd, (char *) &pkt, &buflen, s_dlsap, d_dlsap) < 0) {
	    if (verbose)
		  printf("rcv_ping: do_rcv_unitdata() failed\n");
	    return -1;
      }

      if (gettimeofday(&tv, NULL) < 0) {
	    perror("rcv_ping: gettimeofday()");
	    return -1;
      }

      dsap = dlsap_sap(d_dlsap);
      ssap = dlsap_sap(s_dlsap);
      daddr = dlsap_addr(d_dlsap);
      saddr = dlsap_addr(s_dlsap);

      if (memcmp(daddr, my_addr, addr_len)) {
	    if (verbose > 2)
		  printf("rcv_ping: Reply is not for our address\n");
	    return -1;
      }
      if (memcmp(dsap, my_sap, sap_len)) {
	    if (verbose > 2)
		  printf("rcv_ping: Reply is not for our SAP\n");
	    return -1;
      }
      if (memcmp(saddr, target_addr, addr_len)) {
	    if (verbose > 2)
		  printf("rcv_ping: Packet is not from our target MAC addr.\n");
	    return -1;
      }
      if (in_cksum(&pkt.ip_pkt.ip_hdr, sizeof(struct iphdr)) != 0) {
	    if (verbose)
		  fprintf(stderr, "rcv_ping: Bad IP header checksum\n");
	    return -1;
      }
      if (verbose > 3) {
	    struct in_addr  ss;

	    ss.s_addr = pkt.ip_pkt.ip_hdr.saddr;
	    strcpy(strs, (char *) inet_ntoa(ss));
	    ss.s_addr = pkt.ip_pkt.ip_hdr.daddr;
	    strcpy(strd, (char *) inet_ntoa(ss));
	    printf("rcv_ping: Packet from %s to %s\n", strs, strd);
      }
      if (pkt.ip_pkt.ip_hdr.daddr != my_ip_addr.s_addr) {
	    if (verbose > 2)
		  printf("rcv_ping: Packet is not for my IP addr.\n");
	    return -1;
      }
      if (pkt.ip_pkt.ip_hdr.saddr != target_ip_addr.s_addr) {
	    if (verbose > 2)
		  printf("rcv_ping: Packet is not from our target IP addr.\n");
	    return -1;
      }
      if (pkt.ip_pkt.ip_hdr.protocol != 1 /* ICMP */ ) {
	    if (verbose > 2)
		  printf("rcv_ping: Packet from our target is not type ICMP but %d\n.",
			 (int) pkt.ip_pkt.ip_hdr.protocol);
	    return -1;
      }

      if (in_cksum(&pkt.ip_pkt, ntohs(pkt.ip_pkt.ip_hdr.tot_len)) != 0) {
	    if (verbose)
		  fprintf(stderr, "rcv_ping: Bad ICMP packet checksum\n");
	    dumpbytes("rcv_ping: packet dump:", (char *) &pkt, buflen);
	    return -1;
      }
      if (pkt.ip_pkt.icmp_hdr.type != ICMP_ECHOREPLY) {
	    if (verbose > 2)
		  printf
		      ("rcv_ping: Packet from our target is not ICMP type ECHOREPLY.\n");
	    return -1;
      }
      if (pkt.ip_pkt.icmp_hdr.code != 0) {
	    if (verbose)
		  fprintf(stderr,
			  "rcv_ping: ICMP ECHOREPLY packet from our target is not code 0\n.");
	    return -1;
      }
      if (pkt.ip_pkt.icmp_hdr.un.echo.id != id) {
	    if (verbose > 2)
		  printf
		      ("rcv_ping: ICMP ECHOREPLY packet from our target is not for us\n.");
	    return -1;
      }

      *sequence = pkt.ip_pkt.icmp_hdr.un.echo.sequence;

      if (verbose > 3) {
	    printf("\nIt worked !!!!!\n\n");
	    printf("Received ECHO sequence %d from our target\n", *sequence);
	    rtt = (1000.0 * tv.tv_sec + tv.tv_usec / 1000.0) -
		(1000.0 * pkt.ip_pkt.tv.tv_sec + pkt.ip_pkt.tv.tv_usec / 1000.0);
	    printf("RTT = %6.2f ms\n", rtt);
      }

      return 0;
}

int send_test_xid(int fd, int cmnd)
{
      struct strbuf   databuf;

      if (mac_type == DL_ETHER) {
	    struct eth_llc_hdr frm;	/* 802.2 header */

	    databuf.maxlen = sizeof(frm);
	    databuf.len = sizeof(frm);
	    databuf.buf = (char *) &frm;

	    /*
	     * Fill in header 
	     */
	    memcpy(frm.h_source, my_addr, ETH_ALEN);
	    memcpy(frm.h_dest, dlsap_addr(target_dlsap), addr_len);
	    frm.h_len_hi = 0;
	    frm.h_len_lo = 3;	/* 3 bytes of LLC hdr */
	    frm.llc_dsap = target_sap[0];
	    frm.llc_ssap = my_llc_sap[0];
	    frm.llc_cmnd = cmnd;
      }
      else {
	    unsigned char   trbuf[100];	/* token ring */
	    tr_hdr_t       *tp = (tr_hdr_t *) trbuf;
	    tr_llc_frm_hdr_t *llcp;

	    /*
	     * THIS CODE IS UNTESTED 
	     */
	    databuf.maxlen = sizeof(*tp) - 2 + sizeof(*llcp);
	    databuf.len = sizeof(*tp) - 2 + sizeof(*llcp);
	    databuf.buf = (char *) trbuf;

	    tp->acf = 0x10;	/* canned */
	    tp->fcf = 0x40;	/* canned */
	    memcpy(tp->dst_addr, my_addr, TR_ALEN);
	    memcpy(tp->src_addr, dlsap_addr(target_dlsap), TR_ALEN);
	    tp->src_addr[0] &= ~SADDR_0_RTE_PRES;	/* no route */

	    llcp = (tr_llc_frm_hdr_t *) (((char *) (tp + 1)) - 2);
	    llcp->llc_dsap = target_sap[0];
	    llcp->llc_ssap = my_llc_sap[0];
	    llcp->llc_ctl[0] = cmnd;
      }

      if (verbose > 2) {
	    printf("send_test_xid: Buffer dump RAW data:\n");
	    dumpbuf(databuf);
      }

      if (putmsg(fd, NULL, &databuf, 0) < 0) {
	    perror("send_test_xid: putmsg()");
	    return -1;
      }

      if (verbose > 1)
	    printf("send_test_xid: sent LLC command.\n");
      return 0;
}

int rcv_test_xid(int fd)
{
      int             buflen;
      int             i;
      struct {
	    struct eth_llc_hdr frm __attribute__ ((packed));
	    unsigned char   extra[1024];
      } pkt;
      unsigned char  *saddr,
                     *ssap;
      unsigned char  *daddr,
                     *dsap;
      unsigned char   s_dlsap[MAX_DL_ADDR_LEN + MAX_DL_SAP_LEN];
      unsigned char