[BACK]Return to utils.c CVS log [TXT] [DIR] Up to [ELWIX - Embedded LightWeight unIX -] / ansh / src

Diff for /ansh/src/utils.c between versions 1.1.1.1.2.2 and 1.1.1.1.2.5

version 1.1.1.1.2.2, 2011/10/10 09:11:48 version 1.1.1.1.2.5, 2011/10/13 11:01:37
Line 29  PrepareL2(const char *psDev, int *bpflen) Line 29  PrepareL2(const char *psDev, int *bpflen)
         register int i;          register int i;
         char szStr[STRSIZ];          char szStr[STRSIZ];
         struct ifreq ifr;          struct ifreq ifr;
           struct bpf_program fcode = { 0 };
           struct bpf_insn insns[] = {
                   BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
                   BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ANSH_ID, 0, 1),
                   BPF_STMT(BPF_RET + BPF_K, -1),
                   BPF_STMT(BPF_RET + BPF_K, 0),
           };
   
         FTRACE(3);          FTRACE(3);
         assert(psDev);          assert(psDev);
   
           fcode.bf_len = sizeof(insns) / sizeof(struct bpf_insn);
           fcode.bf_insns = insns;
   
         for (i = 0; i < 10; i++) {          for (i = 0; i < 10; i++) {
                 memset(szStr, 0, sizeof szStr);                  memset(szStr, 0, sizeof szStr);
                 snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);                  snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
Line 45  PrepareL2(const char *psDev, int *bpflen) Line 55  PrepareL2(const char *psDev, int *bpflen)
                 return -1;                  return -1;
         }          }
   
           if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {
                   printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
                   close(h);
                   return -1;
           }
         strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);          strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);
         if (ioctl(h, BIOCSETIF, &ifr) == -1) {          if (ioctl(h, BIOCSETIF, &ifr) == -1) {
                 printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));                  printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
                 close(h);                  close(h);
                 return -1;                  return -1;
         }          }
        if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {        if (ioctl(h, BIOCSETF, &fcode) == -1) {
                printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));                printf("Error:: set filter interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
                 close(h);                  close(h);
                 return -1;                  return -1;
         }          }
Line 62  PrepareL2(const char *psDev, int *bpflen) Line 77  PrepareL2(const char *psDev, int *bpflen)
                 return -1;                  return -1;
         }          }
   
           /*
           n = fcntl(h, F_GETFL);
           fcntl(h, F_SETFL, n | O_NONBLOCK);
           */
   
         VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);          VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
         return h;          return h;
 }  }
Line 217  icmpSend(int s, u_short id, char flg, u_int crypted, u Line 237  icmpSend(int s, u_short id, char flg, u_int crypted, u
         return ret;          return ret;
 }  }
   
intstatic int
pktSend(int s, u_short id, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)_pkt_Send(int s, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
 {  {
         u_char *pos, buf[USHRT_MAX] = { 0 };          u_char *pos, buf[USHRT_MAX] = { 0 };
         struct ether_header *e = (struct ether_header*) buf;          struct ether_header *e = (struct ether_header*) buf;
Line 229  pktSend(int s, u_short id, char flg, u_int crypted, u_ Line 249  pktSend(int s, u_short id, char flg, u_int crypted, u_
         if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)          if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
                 return ANSH_FLG_ERR;                  return ANSH_FLG_ERR;
   
        e->ether_type = htons(id);        e->ether_type = ntohs(ANSH_ID);
         memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);          memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);
         hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);          hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
         pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);          pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
Line 257  pktSend(int s, u_short id, char flg, u_int crypted, u_ Line 277  pktSend(int s, u_short id, char flg, u_int crypted, u_
         return ret;          return ret;
 }  }
   
charint
pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, pktSend(int s, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
                struct ether_header *eth) 
 {  {
        int ret = 0;        int wlen, ret = 0;
         u_char *pos = data;
 
         while (datlen > -1) {
                 wlen = _pkt_Send(s, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
                 if (wlen == -1)
                         return -1;
                 else {
                         pos += wlen;
                         datlen -= wlen;
                         ret += wlen;
                 }
         }
 
         return ret;
 }
 
 static char
 _pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict crypted, 
                 u_char * __restrict data, int * __restrict datlen, 
                 u_char ** __restrict next, int * __restrict nextlen)
 {
         int bias;
         struct bpf_hdr *bpf;          struct bpf_hdr *bpf;
         struct ether_header *e;  
         struct ansh_hdr *hdr;          struct ansh_hdr *hdr;
         u_char *buf;  
         u_int crc;          u_int crc;
   
        if (!eth || !datlen)        if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
                 VERB(1) LOG("Discard packet too short %d ...", rlen);
                 return ANSH_FLG_ERR;                  return ANSH_FLG_ERR;
   
         if (!(buf = malloc(*datlen))) {  
                 ERR("malloc() #%d - %s", errno, strerror(errno));  
                 return ANSH_FLG_ERR;  
         }  
   
         ret = read(s, buf, *datlen);  
         if (ret == -1) {  
                 ERR("Receive packet() #%d - %s", errno, strerror(errno));  
                 free(buf);  
                 return ANSH_FLG_ERR;  
         } else  
                 VERB(4) LOG("Get packet with len=%d", ret);  
   
         /* check header len */  
         if (ret < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {  
                 VERB(1) LOG("Discard packet too short %d ...", ret);  
                 free(buf);  
                 return ANSH_FLG_ERR;  
         } else {          } else {
                 bpf = (struct bpf_hdr*) buf;                  bpf = (struct bpf_hdr*) buf;
                 e = (struct ether_header*) (buf + bpf->bh_hdrlen);  
                 memcpy(eth, e, ETHER_HDR_LEN);  
                 hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);                  hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
         }          }
   
           /* slice readed data to packets */
           if ((bias = BPF_WORDALIGN(bpf->bh_hdrlen + bpf->bh_caplen)) < rlen) {
                   *next = buf + bias;
                   *nextlen = rlen - bias;
           } else {
                   *next = NULL;
                   *nextlen = 0;
           }
   
         /* check version and total size of packet */          /* check version and total size of packet */
         if (hdr->ansh_ver != ANSH_VERSION) {          if (hdr->ansh_ver != ANSH_VERSION) {
                 VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);                  VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
                 free(buf);  
                 return ANSH_FLG_ERR;                  return ANSH_FLG_ERR;
         }          }
         if (crypted) {          if (crypted) {
Line 321  pktRecv(int s, u_int * __restrict crypted, u_char * __ Line 348  pktRecv(int s, u_int * __restrict crypted, u_char * __
         hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));          hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
         if (crc != hdr->ansh_crc) {          if (crc != hdr->ansh_crc) {
                 VERB(3) LOG("Packet with wrong crc ...");                  VERB(3) LOG("Packet with wrong crc ...");
                 free(buf);  
                 return ANSH_FLG_ERR;                  return ANSH_FLG_ERR;
         }          }
   
        /* copy data */        /* select data */
         if (data) {          if (data) {
                 memset(data, 0, *datlen);  
                 *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);                  *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
                memcpy(data, hdr + sizeof(struct ansh_hdr), *datlen);                memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen);
         }          }
   
        ret = (char) hdr->ansh_flg;        return hdr->ansh_flg;
 }
 
 char
 pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, 
                 struct ether_header *eth)
 {
         u_char *buf, *next, *pos, *ptr;
         int nextlen, rlen, buflen, ptrlen;
         char flg;
         struct bpf_hdr *bpf;
         struct ether_header *e;
 
         if (!eth || !datlen)
                 return ANSH_FLG_ERR;
 
         if (!(buf = malloc(*datlen))) {
                 ERR("malloc() #%d - %s", errno, strerror(errno));
                 return ANSH_FLG_ERR;
         }
 
         rlen = read(s, buf, *datlen);
         if (rlen == -1) {
                 ERR("Receive packet() #%d - %s", errno, strerror(errno));
                 free(buf);
                 return ANSH_FLG_ERR;
         } else
                 VERB(4) LOG("Get packet with len=%d", rlen);
 
         /* check header len */
         if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
                 VERB(1) LOG("Discard packet too short %d ...", rlen);
                 free(buf);
                 return ANSH_FLG_ERR;
         } else {
                 bpf = (struct bpf_hdr*) buf;
                 e = (struct ether_header*) (buf + bpf->bh_hdrlen);
                 memcpy(eth, e, ETHER_HDR_LEN);
         }
 
         ptr = next = buf;
         ptrlen = nextlen = rlen;
         pos = data;     
         buflen = *datlen;
         if ((flg = _pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen)) == -1) {
                 free(buf);
                 return ANSH_FLG_ERR;
         } else {
                 pos += buflen;
                 *datlen = buflen;
                 ptr = next;
                 ptrlen = nextlen;
         }
         while (next && nextlen > 0)
                 if (_pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen) == -1)
                         break;
                 else {
                         pos += buflen;
                         *datlen += buflen;
                         ptr = next;
                         ptrlen = nextlen;
                 }
 
         free(buf);          free(buf);
        return (char) ret;        return flg;
 }  }
   
 void *  void *
Removed from v.1.1.1.1.2.2  
changed lines
  Added in v.1.1.1.1.2.5

FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>