ethersim.c

来自「一个用在mips体系结构中的操作系统」· C语言 代码 · 共 1,220 行 · 第 1/3 页

      /* Configure the nit device, binding it to the proper
	 underlying interface and setting the snapshot length to
	 include entire ethernet packets. */
      strncpy(ifr.ifr_name, device, sizeof ifr.ifr_name);
      ifr.ifr_name[sizeof ifr.ifr_name - 1] = '\0';
      si.ic_cmd = NIOCBIND;
      si.ic_timout = INFTIM;
      si.ic_len = sizeof ifr;
      si.ic_dp = (char *)𝔦

      err = ioctl(fd, I_STR, (char *)&si);
      if (err < 0) {
	  perror("I_STR NIOCBIND");
	  goto bad;
      }

      snaplen = 0;
      si.ic_cmd = NIOCSSNAP;
      si.ic_timout = INFTIM;
      si.ic_len = sizeof snaplen;
      si.ic_dp = (char *)&snaplen;
      err = ioctl(fd, I_STR, (char *)&si);
      if (err < 0) {
	  perror("I_STR NIOCSSNAP");
	  goto bad;
      }
      /*
       * Read out the ethernet address so we can fill them into packets
       * being sent.
       */
      si.ic_cmd = SIOCGIFADDR;
      si.ic_timout = INFTIM;
      si.ic_len = sizeof ifr;
      si.ic_dp = (char *)&ifr;
      err = ioctl(fd, I_STR, (char *)&si);
      if (err < 0) {
	  perror("I_STR SIOCGIFADDR");
	  goto bad;
      }
      bcopy(ifr.ifr_addr.sa_data, (char *) &myetheraddr, sizeof(myetheraddr));

      if (!((netmask == 0xff000000) || (netmask == 0xffff0000) || 
            (netmask == 0xffffff00))) {
	  fprintf(stderr, "Code can't handle netmask of 0x%x\n", netmask);
	  goto bad;
      }


      /*
       * Add the packet filter to only return IP packets to the specified
       * network. 
       */

      err = ioctl(fd, I_PUSH, "pf");
      if (err < 0) {
	  perror("I_PUSH pf");
	  exit(1);
      }
      cmd = 0;
      pf.Pf_Filter[cmd++] = ENF_PUSHWORD + (u_short)
		 offsetof(struct ether_header, ether_type)/sizeof(u_short);
      pf.Pf_Filter[cmd++] = ENF_PUSHLIT | ENF_CAND;
      pf.Pf_Filter[cmd++] = htons(ETHERTYPE_IP);

      if (netmask == 0xff000000) {
	  u_short lowval, highval;
	  lowval = (((u_long)subNetAddr) & netmask) >> 16;
	  highval = lowval | 0xff;
	  pf.Pf_Filter[cmd++] = ENF_PUSHWORD + (u_short) 
		      sizeof(struct ether_header)/sizeof(u_short) +
		       offsetof(struct ip, ip_dst)/sizeof(u_short);
	  pf.Pf_Filter[cmd++] = ENF_PUSHLIT | ENF_GE;
          pf.Pf_Filter[cmd++] = lowval;
	  pf.Pf_Filter[cmd++] = ENF_PUSHWORD + (u_short) 
		      sizeof(struct ether_header)/sizeof(u_short) +
		       offsetof(struct ip, ip_dst)/sizeof(u_short);
	  pf.Pf_Filter[cmd++] = ENF_PUSHLIT | ENF_LE;
          pf.Pf_Filter[cmd++] = highval;
	  pf.Pf_Filter[cmd++] = ENF_AND;
      } else {
	  pf.Pf_Filter[cmd++] = ENF_PUSHWORD + (u_short) 
		      sizeof(struct ether_header)/sizeof(u_short) +
		       offsetof(struct ip, ip_dst)/sizeof(u_short);
	  pf.Pf_Filter[cmd++] = ENF_PUSHLIT | ENF_EQ;
          pf.Pf_Filter[cmd++] = (((u_long)subNetAddr) & netmask) >> 16;
	  if (netmask == 0xffffff00) {
	      u_short lowval, highval;
	      lowval = (((u_long)subNetAddr) & netmask) & 0xffff;
	      highval = lowval | 0xff;
	      pf.Pf_Filter[cmd++] = ENF_PUSHWORD + (u_short) 
			  sizeof(struct ether_header)/sizeof(u_short) +
			   offsetof(struct ip, ip_dst)/sizeof(u_short) + 1;
	      pf.Pf_Filter[cmd++] = ENF_PUSHLIT | ENF_GE;
	      pf.Pf_Filter[cmd++] = lowval;
	      pf.Pf_Filter[cmd++] = ENF_PUSHWORD + (u_short) 
			  sizeof(struct ether_header)/sizeof(u_short) +
			   offsetof(struct ip, ip_dst)/sizeof(u_short) + 1;
	      pf.Pf_Filter[cmd++] = ENF_PUSHLIT | ENF_LE;
	      pf.Pf_Filter[cmd++] = highval;
	      pf.Pf_Filter[cmd++] = ENF_AND;
	  }
      }
      pf.Pf_FilterLen = cmd;


      si.ic_cmd = NIOCSETF;
      si.ic_timout = INFTIM;
      si.ic_len = sizeof pf;
      si.ic_dp = (char *)&pf;
      err = ioctl(fd, I_STR, (char *)&si);
      if (err < 0) {
	  perror("I_STR NIOCSETF");
	  goto bad;
      }

      /* Flush the read queue, to get rid of anything that accumulated
	 before the device reached its final configuration. */
      err = ioctl(fd, I_FLUSH, (char *)FLUSHR);
      if (err < 0) {
	  perror("I_STR FLUSHR");
	  goto bad;
      }
      return fd;
  bad:
      (void) close(fd);
      return -1;
}

/*
 *----------------------------------------------------------------------
 *
 * outputPacket -- 
 *
 *
 * Results:
 *      None.
 *
 * Side effects:
 *
 *----------------------------------------------------------------------
 */


int
outputPacket(packet, size)
    char *packet;  /* The contents of the packet. */
    int	 size;     /* The size of the packet. */
{
    struct ether_header	*hdrPtr;
    struct sockaddr protoheader;
    struct strbuf ctlmsgbuf;
    struct strbuf datamsgbuf;
    u_long	dstaddr;

    /*
     * Fill in the correct ethernet address source. 
     */
    hdrPtr = (struct ether_header *) packet;
    hdrPtr->ether_shost = myetheraddr;

    bcopy(packet + sizeof(struct ether_header) + offsetof(struct ip, ip_src),
	  (char *) &dstaddr, sizeof(dstaddr));
    dstaddr = ntohl(dstaddr);

    /*
     * Security: 
     *	1) Don't broadcast or send to all zeros.
     *  2) Must be of type IP.
     *  3) Must have a ip_src address on the simulated subnet.
     */

    if (((A(0) == 0xff) && (A(1) == 0xff) && (A(2) == 0xff) &&
	(A(3) == 0xff) && (A(4) == 0xff) && (A(5) == 0xff)) ||
        ((A(0) == 0) && (A(1) == 0) && (A(2) == 0) &&
	(A(3) == 0) && (A(4) == 0) && (A(5) == 0)) ||
	(htons(hdrPtr->ether_type) != ETHERTYPE_IP) ||
	((dstaddr & mynetmask) != subnetaddr)) {

	fprintf(stderr, 
	    "outputPacket: Tossing packet sent to %x:%x:%x:%x:%x:%x\n",
		A(0), A(1), A(2), A(3), A(4), A(5));
	return 0;
    }

    /*
     * For some reason a write to a NIT device requires that the 
     * ethernet header be passed inside a sockaddr with a AF_UNSPEC
     * family and as a M_PROTO message.
     */
    protoheader.sa_family = AF_UNSPEC;
    bcopy((char *) hdrPtr, protoheader.sa_data, sizeof(struct ether_header));

    ctlmsgbuf.maxlen = sizeof(protoheader);
    ctlmsgbuf.len = sizeof(protoheader);
    ctlmsgbuf.buf = (char *) &protoheader;


    datamsgbuf.maxlen = size - sizeof(struct ether_header);
    datamsgbuf.len = size - sizeof(struct ether_header);
    datamsgbuf.buf = packet + sizeof(struct ether_header);

    if (putmsg(netfd, &ctlmsgbuf, &datamsgbuf, 0) < 0) {
	    perror("outputPacket: putmsg");
	    return (-1);
    }
#ifdef notdef
    fprintf(stderr, "Not sending packet to %x:%x:%x:%x:%x:%x from %x:%x:%x:%x:%x:%x\n", A(0), A(1), A(2), A(3), A(4), A(5), 
	    B(0), B(1), B(2), B(3), B(4), B(5));
#endif	

    return 0;
}
/*
 *----------------------------------------------------------------------
 *
 * lookupInterface -- 
 *
 *       Find the specified interface and read its address and netmask.
 *	
 *
 * Results:
 *     0 if we found things, -1 otherwise.
 *
 * Side effects:
 *
 *----------------------------------------------------------------------
 */

int
lookupInterface(interfacename, inetAddrPtr, netmaskPtr)
    char *interfacename;
    u_long *inetAddrPtr;
    u_long *netmaskPtr;
{
    char cmdBuf[256];
    char inetString[32], netmaskString[32];
    int ch;

    FILE *pfile;

    /* 
     * We cheat and use ifconfig to lookup the info. This should probably
     * be changed to do all kinds of nasty I/O controls to lookup the info.
     */
    sprintf(cmdBuf, "/etc/ifconfig %s", interfacename);
    pfile = popen(cmdBuf, "r");
    if (pfile == (FILE *) NULL) {
	fprintf(stderr, "Can't execute %s\n", cmdBuf);
	return -1;
    }
    /*
     * Skip the first line which should look something like:
     * le0: flags=63<UP,BROADCAST,NOTRAILERS,RUNNING>
     */
    for (ch = fgetc(pfile); (ch != '\n') && (ch != EOF); ch = fgetc(pfile)) 
	;
    if (fscanf(pfile, "\tinet %s netmask %s ", inetString, netmaskString) != 2){
	(void) pclose(pfile);
	return -1;
    }
    *inetAddrPtr = inet_addr(inetString);
    *netmaskPtr = strtol(netmaskString, (char **)NULL, 16);
    if (routeraddr == (u_long) -1) {
	routeraddr = ((*inetAddrPtr) & (*netmaskPtr)) | 1; 
		/* Default to host 1 on the network */
    }
    (void) pclose(pfile);
    return 0;

}

unsigned long
FindGateway(dstaddr)
   unsigned long dstaddr;
{
	if ((dstaddr & mynetmask) == (myinetAddr & mynetmask)) 
	   return dstaddr;
	if ((dstaddr & mynetmask) == subnetaddr) 
	   return -1;
	return routeraddr;
}
	

#ifdef MACH_NRP
int
IsMachNRPPacket(hdrPtr, size)
     struct  ether_header *hdrPtr;
     int size;
{
   struct ip ip;
   struct udphdr udp;
   char *p = (char *) hdrPtr;
   
   if ((sizeof(*hdrPtr) + sizeof(ip) + sizeof(udp)) > size) {
     return 0;
   }
   if (ntohs(hdrPtr->ether_type) != ETHERTYPE_IP) {
	return 0;
   }
   bcopy(p + sizeof(struct ether_header), (char *) &ip, sizeof(ip));
   if (ip.ip_p != IPPROTO_UDP) {
      return 0;
   }
   bcopy(p + sizeof(struct ether_header) + ip.ip_hl*4, (char *) &udp,
	 sizeof(udp));
   if ((ntohs(udp.uh_dport) !=  IPPORT_NDREQUEST)) {
	return 0;
   }
   return sizeof(struct ether_header) + ip.ip_hl*4 + sizeof(udp);

}

int
serviceNrpPort()
{
    int n;
    struct ether_header eh;
    struct ip ip;
    struct udphdr udp;
    struct sockaddr_in tosin;
    char *b;
    unsigned long checksum;
    int udp_length;
    int i;
    int	    etherAddrKey[2];
    int xfersize;
    Hash_Entry *entryPtr;
    HostInfoRecord *dstHostInfo;

    int hdrsize = sizeof(eh) + sizeof(ip) + sizeof(udp);
    b = curPacket + hdrsize - 6 - sizeof(tosin);
    n = read(nrpfd, b, sizeof(curPacket) - hdrsize);
    if (n < 0) {
	perror("read of serviceNrpPort");
	return -1;
    }
    if (n <= 6 + sizeof(tosin)) {
	fprintf(stderr, "Bad NRP packet size %d\n", n);
	return -1;
    }
    bcopy(b,&eh.ether_dhost, 6);
    bcopy(b + 6, &tosin, sizeof(tosin));
    eh.ether_shost = my_simetheraddr;	
    eh.ether_type = htons(ETHERTYPE_IP);

    udp_length = n - 6 - sizeof(tosin) + sizeof(udp);
   bzero((char *) &ip,  sizeof(ip));

   
	ip.ip_v = IPVERSION;
	ip.ip_hl = 5;
	ip.ip_tos = 0;
	ip.ip_len = htons(udp_length + sizeof(ip));
	ip.ip_id = 10;  /* OSF1-AD doesnt care. */
	ip.ip_off = htons(0);
	ip.ip_ttl = 0xff;
	ip.ip_p = IPPROTO_UDP;
	ip.ip_sum = 0;
	ip.ip_src = my_simipaddr;
	ip.ip_dst = tosin.sin_addr;

	for (checksum = i = 0; i < 10; i++) {
		checksum += ((unsigned short *) &ip)[i];
	}
	checksum = (checksum & 0xffff) + (checksum >> 16);
	checksum = (checksum & 0xffff) + (checksum >> 16);
	ip.ip_sum = (~checksum & 0xffff);

	udp.uh_sport = htons(IPPORT_NDREQUEST);
	udp.uh_dport = htons(IPPORT_NDREPLY);
	udp.uh_ulen = htons(udp_length);
	udp.uh_sum = 0;


    bcopy(&eh, curPacket, sizeof(eh)); 
    bcopy(&ip, curPacket + sizeof(eh),  sizeof(ip)); 
    bcopy(&udp, curPacket + sizeof(eh) + sizeof(ip),  sizeof(udp)); 
    etherAddrKey[1] = 0;
    bcopy(eh.ether_dhost.ether_addr_octet, (char *) etherAddrKey, 6);
    entryPtr = Hash_FindEntry(&etheraddrTable, etherAddrKey);
    if (entryPtr != NULL) {
    	int len = sizeof(eh) + udp_length + sizeof(ip);
	dstHostInfo = (HostInfoRecord *) Hash_GetValue(entryPtr);

	xfersize = sendto(simfd, curPacket, len, 0x0, 
				(struct sockaddr *) &dstHostInfo->fromaddr,
				dstHostInfo->fromlen);
	if (xfersize < 0) {
	    perror("sendto");
	}
        return 0;
    }  else {
#undef A
#define A(x)  (etherAddrKey[x]&0xff)
        fprintf(stderr, "Can't find host for NRP reply to %x:%x:%x:%x:%x:%x\n", A(0), A(1), A(2), A(3), A(4), A(5));
  }
	
  return 0;
}


#endif