ldltest.c

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

      return 0;
}

/**************************************************************/
/*
 */
/*
 * IP address resolution 
 */
/*
 */
/**************************************************************/

/*
 *  Send an ARP request
 */
int send_arp_request(int fd)
{
      struct arphdr  *arp;
      unsigned char  *p,
                      databuf[1024];
      char            sap[2],
                      dlsap[11];

      assert(addr_len == ETH_ALEN);
      assert(sap_len == 2);

      arp = (struct arphdr *) databuf;
      arp->ar_hrd = htons(hw_type);
      arp->ar_pro = htons(ETH_P_IP);
      arp->ar_hln = addr_len;
      arp->ar_pln = 4;
      arp->ar_op = htons(ARPOP_REQUEST);
      p = (char *) (arp + 1);
      memcpy(p, my_addr, addr_len);
      p += addr_len;
      memcpy(p, &my_ip_addr, sizeof(struct in_addr));
      p += sizeof(struct in_addr);
      memset(p, 0, addr_len);
      p += addr_len;
      memcpy(p, &target_ip_addr, sizeof(struct in_addr));
      p += sizeof(struct in_addr);

      *(unsigned short *) sap = ETH_P_ARP;
      build_dlsap(dlsap, my_brd_addr, sap);

      return do_send_unitdata(fd, databuf, p - databuf, dlsap);
}

int process_arp_reply(int fd, char *databuf, int datalen, char *dlsap)
{
      struct arphdr  *arp;
      unsigned char  *pr;

      if (verbose > 2)
	    printf("process_arp_reply() entered\n");

      if (target_ip_addr.s_addr == 0) {
	    if (verbose > 2)
		  printf("process_arp_reply(): Ignoring reply.\n");
	    return 0;
      }

      arp = (struct arphdr *) databuf;
      if (arp->ar_hrd != htons(hw_type) ||
	  arp->ar_pro != htons(ETH_P_IP) ||
	  arp->ar_hln != addr_len || arp->ar_pln != 4
	  || arp->ar_op != htons(ARPOP_REPLY)) {
	    if (verbose > 2)
		  printf("process_arp_reply(): Not for me\n");
	    return 0;		/* Not an IP request */
      }

      pr = (char *) (arp + 1);
      if (memcmp(pr + addr_len, &target_ip_addr, 4)) {
	    if (verbose > 2) {
		  struct in_addr  ip_addr;

		  printf
		      ("process_arp_reply(): ARP reply is not for target IP address (%s)",
		       inet_ntoa(target_ip_addr));
		  memcpy(&ip_addr, pr + addr_len, sizeof(ip_addr));
		  printf(", but for %s\n", inet_ntoa(ip_addr));
	    }
	    return 0;		/* Not for my address */
      }

      memcpy(target_addr, pr, addr_len);

      if (verbose > 2)
	    dumpbytes("Got ARP reply: Target hardware address", target_addr, addr_len);

      got_target_addr = 1;
      kill(getpid(), SIGALRM);

      return 0;
}

int process_arp_request(int fd, char *databuf, int datalen, char *dlsap)
{
      struct arphdr  *arp;
      unsigned char  *pr;

      /*
       * if (verbose > 2) 
       */
      if (verbose)
	    printf("process_arp_request() entered\n");

      if (!arp_respond)
	    return 0;

      arp = (struct arphdr *) databuf;
      if (arp->ar_hrd != htons(hw_type) ||
	  arp->ar_pro != htons(ETH_P_IP) ||
	  arp->ar_hln != addr_len || arp->ar_pln != 4
	  || arp->ar_op != htons(ARPOP_REQUEST)) {
	    if (verbose > 2)
		  printf("process_arp_request(): Not for me\n");
	    return 0;
      }

      pr = (char *) (arp + 1);
      if (memcmp(pr + 2 * addr_len + 4, &my_ip_addr, 4)) {
	    if (verbose > 2) {
		  struct in_addr  ip_addr;

		  printf("process_arp_request(): Not for my address (%s)",
			 inet_ntoa(my_ip_addr));
		  memcpy(&ip_addr, pr + 2 * addr_len + 4, sizeof(ip_addr));
		  printf(", but for %s\n", inet_ntoa(ip_addr));
	    }
	    return 0;		/* Not for my address */
      }

      /*
       * Create ARP reply 
       */
      memcpy(pr + addr_len + 4, my_addr, addr_len);
      arp->ar_op = htons(ARPOP_REPLY);
      {
	    int             i;
	    char           *a = pr,
	                   *b = pr + addr_len + 4;

	    for (i = 0; i < addr_len + 4; ++i) {
		  char            c = a[i];

		  a[i] = b[i];
		  b[i] = c;
	    }
      }

      if (verbose)
	    printf("process_arp_request() replying\n");
      return do_send_unitdata(fd, databuf, datalen, dlsap);
}

int process_incoming_arp(int fd, char *databuf, int datalen, char *dlsap)
{
      struct arphdr  *arp;

      if (verbose > 2)
	    printf("process_incoming_arp() entered\n");

      if (datalen < sizeof(struct arphdr)) {
	    if (verbose)
		  fprintf(stderr, "process_incoming_arp(): Received short ARP frame\n");
	    return -1;
      }

      arp = (struct arphdr *) databuf;
      if (arp->ar_hrd != htons(hw_type) || arp->ar_pro != htons(ETH_P_IP)
	  || arp->ar_hln != addr_len || arp->ar_pln != 4) {
	    if (verbose > 2)
		  printf("process_incoming_arp(): Not for me.\n");
	    return 0;		/* Not an IP request */
      }

      if (datalen < sizeof(struct arphdr) + 2 * (addr_len + 4)) {
	    if (verbose)
		  fprintf(stderr, "process_incoming_arp(): Received short ARP frame\n");
	    return -1;
      }

      switch (ntohs(arp->ar_op)) {
      case ARPOP_REPLY:
	    return process_arp_reply(fd, databuf, datalen, dlsap);
      case ARPOP_REQUEST:
	    return process_arp_request(fd, databuf, datalen, dlsap);
      default:
	    if (verbose)
		  printf
		      ("process_incoming_arp(): Ignoring unknown ARP operation code 0x%04x\n",
		       ntohs(arp->ar_op));
	    return -1;
      }
}

int process_incoming_echo_request(int fd, char *databuf, int datalen, char *dlsap)
{
      struct ping_pkt *pkt = (struct ping_pkt *) databuf;
      __u32           addr;

      if (!echo_respond)
	    return 0;

      /*
       *  Sanity checks
       */
      if (in_cksum(&pkt->icmp_hdr, ntohs(pkt->ip_hdr.tot_len) - sizeof(pkt->ip_hdr)) != 0) {
	    if (verbose)
		  fprintf(stderr,
			  "process_incoming_echo_request: Bad ICMP packet checksum\n");
	    dumpbytes("process_incoming_echo_request: packet dump:", databuf, datalen);
	    return -1;
      }
      assert(pkt->icmp_hdr.type == ICMP_ECHO);
      if (pkt->icmp_hdr.code != 0) {
	    if (verbose)
		  fprintf(stderr,
			  "process_incoming_echo_request: ICMP ECHOREQUEST packet is not code 0\n.");
	    return -1;
      }

      /*
       *  Build reply
       */
      pkt->ip_hdr.ttl = 4;
      pkt->ip_hdr.check = 0;
      addr = pkt->ip_hdr.daddr;
      pkt->ip_hdr.daddr = pkt->ip_hdr.saddr;
      pkt->ip_hdr.saddr = addr;
      pkt->ip_hdr.check = in_cksum(&pkt->ip_hdr, sizeof(struct iphdr));

      pkt->icmp_hdr.type = ICMP_ECHOREPLY;
      pkt->icmp_hdr.code = 0;
      pkt->icmp_hdr.checksum = 0;
      pkt->icmp_hdr.checksum =
	  in_cksum(&pkt->icmp_hdr, ntohs(pkt->ip_hdr.tot_len) - sizeof(pkt->ip_hdr));

      if (verbose > 2)
	    printf("process_incoming_echo_request: Sending ICMP ECHO reply.\n");

      /*
       *  Send reply
       */
      return do_send_unitdata(fd, databuf, datalen, dlsap);
}

int process_incoming_llc_request(int fd, char *databuf, int datalen,
				 unsigned char *src_dlsap, unsigned char *dst_dlsap)
{
      unsigned        cmnd;

      if (verbose > 2) {
	    char           *p;
	    int             n;

	    printf("process_incoming_llc_request: length=%d\n", datalen);
	    for (p = databuf, n = datalen; n > 0; p++, n--)
		  printf("%02x ", *p & 0xff);
	    printf("\n");
      }
      else if (verbose > 1)
	    printf("process_incoming_llc_request: "
		   "%d bytes: %02x %02x %02x\n", datalen, databuf[0] & 0xFF,
		   databuf[1] & 0xFF, databuf[2] & 0xFF);

      if (databuf[1] & 0x01)	/* it's a response */
	    return (1);		/* only respond to commands */

      switch ((databuf[2] & ~0x10) & 0xFF) {
      case 0xE3:		/* TEST */
	    cmnd = databuf[2];
	    break;
      case 0xAF:		/* XID */
	    cmnd = databuf[2];
	    break;
      case 0x43:		/* DISC */
      case 0x6F:		/* SABME */
	    cmnd = 0x0F | (databuf[2] & 0x10);	/* DM, possibly w/F-bit */
	    break;
      default:
	    return (0);		/* no response */
      }

      memcpy(my_addr, dst_dlsap, ETH_ALEN);	/* dst addr is my addr */
      memcpy(target_dlsap, src_dlsap, ETH_ALEN);	/* src addr is remote addr */
      target_sap[0] = databuf[1];	/* trgt sap is ssap */
      my_llc_sap[0] = databuf[0] | 0x01;	/* my sap is dsap */
      send_test_xid(fd, cmnd);	/* return response */
      return (-1);		/* responded to */
}

int _do_rcv_unitdata(int fd, unsigned char *data, int *datalen, unsigned char *src_dlsap,
		     unsigned char *dst_dlsap)
{
      struct strbuf   ctlbuf,
                      databuf;
      char            buf[1024];
      dl_unitdata_ind_t *reply;
      int             flags,
                      ret;
      unsigned short  pkt_type = 0;

      ctlbuf.maxlen = sizeof(buf);
      ctlbuf.len = 0;
      ctlbuf.buf = buf;
      databuf.maxlen = *datalen;
      databuf.len = 0;
      databuf.buf = data;

      flags = 0;
      if (verbose > 1)
	    printf("_do_rcv_unitdata(): Waiting for data...\n");
      ret = getmsg(fd, &ctlbuf, &databuf, &flags);
      if (ret < 0) {
	    perror("_do_rcv_unitdata(): getmsg()");
	    return -1;
      }
      if (ret & MORECTL) {
	    struct strbuf   ctlbuf2;
	    char            buf2[1024];
	    int             flags2,
	                    ret2;

	    if (verbose > 2)
		  printf("_do_rcv_unitdata(): More control data: ignored\n");
	    do {
		  ctlbuf2.maxlen = sizeof(buf2);
		  ctlbuf2.len = 0;
		  ctlbuf2.buf = buf2;
		  flags2 = 0;
		  ret2 = getmsg(fd, &ctlbuf2, NULL, &flags2);
		  if (ret2 < 0) {
			perror("_do_rcv_unitdata(): getmsg()");
			return -1;
		  }
	    }
	    while (ret2 & MORECTL);
      }
      if (ret & MOREDATA) {
	    struct strbuf   databuf2;
	    char            buf2[1024];
	    int             flags2,
	                    ret2;

	    if (verbose > 2)
		  printf("_do_rcv_unitdata(): More data: ignored\n");
	    do {
		  databuf2.maxlen = sizeof(buf2);
		  databuf2.len = 0;
		  databuf2.buf = buf2;
		  flags2 = 0;
		  ret2 = getmsg(fd, NULL, &databuf2, &flags2);
		  if (ret2 < 0) {
			perror("_do_rcv_unitdata(): getmsg()");
			return -1;
		  }
	    }
	    while (ret2 & MOREDATA);
      }

      if (verbose > 2) {
	    printf("_do_rcv_unitdata(): Buffer dump ctl:\n");
	    dumpbuf(ctlbuf);
	    printf("\n_do_rcv_unitdata(): Buffer dump data:\n");
	    dumpbuf(databuf);
      }

      if (ctlbuf.len <= 0 && framing == LDL_FRAME_RAW_LLC) {
	    switch (mac_type) {	/* DLPI MAC type */
	    case DL_ETHER:	/* Ethernet */
		  /*
		   * Extract dst and src addrs.  Tack saps onto the
		   * end of the respective addresses.  Trim addresses
		   * and saps from data buffer.
		   */
		  memcpy(dst_dlsap, &databuf.buf[0], ETH_ALEN);
		  dst_dlsap[ETH_ALEN] = databuf.buf[2 * ETH_ALEN];
		  dst_dlsap[ETH_ALEN + 1] = databuf.buf[2 * ETH_ALEN + 1];
		  dst_dlsap[ETH_ALEN + 2] = databuf.buf[2 * ETH_ALEN + 2];
		  memcpy(src_dlsap, &databuf.buf[ETH_ALEN], ETH_ALEN);
		  src_dlsap[ETH_ALEN] = databuf.buf[2 * ETH_ALEN + 2 + 1];
		  pkt_type = ntohs(*((unsigned short *) &databuf.buf[2 * ETH_ALEN]));
		  if (pkt_type < 1536)	/* llc type */
			databuf.len = pkt_type;
		  else		/* IP or other */
			databuf.len -= (2 * ETH_ALEN + 2);

		  memcpy(&databuf.buf[0], &databuf.buf[2 * ETH_ALEN + 2], databuf.len);
		  addr_len = ETH_ALEN;
		  sap_len = 1;
		  break;
	    case DL_TPR:	/* Token Ring */
		  {
			unsigned        rtelgth = 0;
			tr_hdr_t       *tp = (tr_hdr_t *) databuf.buf;
			tr_llc_frm_hdr_t *llcp;

			/*
			 * THIS CODE IS UNTESTED 
			 */
			memcpy(dst_dlsap, tp->dst_addr, TR_ALEN);
			memcpy(src_dlsap, tp->src_addr, TR_ALEN);
			if ((databuf.buf[1] & 0xC0) != 0x40)	/* not LLC */
			      break;

			if (tp->src_addr[0] & SADDR_0_RTE_PRES) {	/* has source
									 * route field */
			      rtelgth = tp->bl & RCF_0_LLLLL;
			      if ((rtelgth < 2)	/* count includes rte ctl fld */
				  ||(rtelgth & 0x01)	/* must be pairs of bytes */
				  )
				    break;	/* ill-formed */
			}

			llcp =
			    (tr_llc_frm_hdr_t *) (databuf.buf + sizeof(*tp) - 2 +
						  rtelgth);
			dst_dlsap[TR_ALEN] = llcp->llc_dsap;
			src_dlsap[TR_ALEN] = llcp->llc_ssap;
			databuf.len -= (sizeof(*tp) - 2 + rtelgth);
			memcpy(&databuf.buf[0], llcp, databuf.len);
			addr_len = TR_ALEN;
			sap_len = 1;
		  }
		  break;
	    }
      }
      else {
	    if (ctlbuf.len < sizeof(dl_unitdata_ind_t) + 2 * (addr_len + sap_len)) {
		  if (ctlbuf.len == 0) {
			if (verbose)
			      printf("_do_rcv_unitdata(): "
				     "No ctlbuf data, probably leftover data\n");
		  }
		  else {
			if (verbose)
			      fprintf(stderr, "_do_rcv_unitdata(): "
				      "Bad ctlbuf.len=%d returned, "
				      "should be at least %d\n",
				      ctlbuf.len,
				      sizeof(dl_unitdata_ind_t) + 2 * (addr_len +
								       sap_len));
		  }
		  return -1;
	    }

	    reply = (dl_unitdata_ind_t *) buf;

	    memcpy(dst_dlsap, ctlbuf.buf + reply->dl_dest_addr_offset,
		   addr_len + sap_len);
	    memcpy(src_dlsap, ctlbuf.buf + reply->dl_src_addr_offset, addr_len + sap_len);

      }
      *datalen = databuf.len;
      if (verbose > 2) {