--- ansh/src/utils.c 2011/10/10 14:15:40 1.1.1.1.2.4 +++ ansh/src/utils.c 2011/10/13 11:01:37 1.1.1.1.2.5 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ - * $Id: utils.c,v 1.1.1.1.2.4 2011/10/10 14:15:40 misho Exp $ + * $Id: utils.c,v 1.1.1.1.2.5 2011/10/13 11:01:37 misho Exp $ * *************************************************************************/ #include "global.h" @@ -55,21 +55,14 @@ PrepareL2(const char *psDev, int *bpflen) return -1; } - strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name); - if (ioctl(h, BIOCSETIF, &ifr) == -1) { - printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno)); - close(h); - return -1; - } - /* - if (ioctl(h, BIOCSHDRCMPLT, &n) == -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; } - */ - if (ioctl(h, BIOCIMMEDIATE, &n) == -1) { - printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno)); + strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name); + if (ioctl(h, BIOCSETIF, &ifr) == -1) { + printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno)); close(h); return -1; } @@ -244,8 +237,8 @@ icmpSend(int s, u_short id, char flg, u_int crypted, u return ret; } -int -pktSend(int s, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea) +static int +_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 }; struct ether_header *e = (struct ether_header*) buf; @@ -284,49 +277,56 @@ pktSend(int s, char flg, u_int crypted, u_char *data, return ret; } -char -pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, - struct ether_header *eth) +int +pktSend(int s, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea) { - 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 ether_header *e; struct ansh_hdr *hdr; - u_char *buf; 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; - - 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 { 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); } + /* 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 */ if (hdr->ansh_ver != ANSH_VERSION) { VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver); - free(buf); return ANSH_FLG_ERR; } if (crypted) { @@ -348,20 +348,80 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __ hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); if (crc != hdr->ansh_crc) { VERB(3) LOG("Packet with wrong crc ..."); - free(buf); return ANSH_FLG_ERR; } - /* copy data */ + /* select data */ if (data) { - memset(data, 0, *datlen); *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr); 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); - return (char) ret; + return flg; } void *