version 1.1.1.1.2.2, 2011/10/10 09:11:48
|
version 1.1.1.1.2.6, 2011/10/13 14:29:30
|
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 101 PrepareL3(const struct sockaddr *sa, int *bpflen)
|
Line 121 PrepareL3(const struct sockaddr *sa, int *bpflen)
|
} |
} |
|
|
char |
char |
icmpRecv(int s, u_short * __restrict id, u_int * __restrict crypted, u_char * __restrict data, | icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted, |
int * __restrict datlen, struct sockaddr *sa, socklen_t *salen) | u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen) |
{ |
{ |
int ret = 0; |
int ret = 0; |
struct icmp *icmp; |
struct icmp *icmp; |
Line 165 icmpRecv(int s, u_short * __restrict id, u_int * __res
|
Line 185 icmpRecv(int s, u_short * __restrict id, u_int * __res
|
memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen); |
memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen); |
} |
} |
|
|
|
if (seq) |
|
*seq = ntohl(hdr->ansh_seq); |
if (id) |
if (id) |
*id = ntohs(icmp->icmp_id); |
*id = ntohs(icmp->icmp_id); |
return hdr->ansh_flg; |
return hdr->ansh_flg; |
} |
} |
|
|
int |
int |
icmpSend(int s, u_short id, char flg, u_int crypted, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen) | icmpSend(int s, u_int seq, u_short id, char flg, u_int crypted, u_char *data, int datlen, |
| struct sockaddr *sa, socklen_t salen) |
{ |
{ |
u_char *pos, buf[USHRT_MAX] = { 0 }; |
u_char *pos, buf[USHRT_MAX] = { 0 }; |
struct icmp *icmp; |
struct icmp *icmp; |
Line 192 icmpSend(int s, u_short id, char flg, u_int crypted, u
|
Line 215 icmpSend(int s, u_short id, char flg, u_int crypted, u
|
hdr->ansh_flg = flg; |
hdr->ansh_flg = flg; |
hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); |
hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); |
hdr->ansh_nonce = htonl(crypted); |
hdr->ansh_nonce = htonl(crypted); |
|
hdr->ansh_seq = htonl(seq); |
hdr->ansh_crc = 0; |
hdr->ansh_crc = 0; |
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); |
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); |
|
|
Line 217 icmpSend(int s, u_short id, char flg, u_int crypted, u
|
Line 241 icmpSend(int s, u_short id, char flg, u_int crypted, u
|
return ret; |
return ret; |
} |
} |
|
|
int | static 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, u_int seq, 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 253 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 240 pktSend(int s, u_short id, char flg, u_int crypted, u_
|
Line 264 pktSend(int s, u_short id, char flg, u_int crypted, u_
|
hdr->ansh_flg = flg; |
hdr->ansh_flg = flg; |
hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); |
hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); |
hdr->ansh_nonce = htonl(crypted); |
hdr->ansh_nonce = htonl(crypted); |
|
hdr->ansh_seq = htonl(seq); |
hdr->ansh_crc = 0; |
hdr->ansh_crc = 0; |
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); |
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); |
|
|
Line 257 pktSend(int s, u_short id, char flg, u_int crypted, u_
|
Line 282 pktSend(int s, u_short id, char flg, u_int crypted, u_
|
return ret; |
return ret; |
} |
} |
|
|
char | int |
pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, | pktSend(int s, u_int seq, 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, seq, 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 seq, 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 353 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; | if (seq) |
| *seq = ntohl(hdr->ansh_seq); |
| return hdr->ansh_flg; |
| } |
| |
| char |
| pktRecv(int s, u_int * __restrict seq, u_int * __restrict crypted, u_char * __restrict data, |
| int * __restrict datlen, struct ether_header *eth) |
| { |
| u_char *buf, *next, *ptr, *pos = data; |
| int nextlen, rlen, buflen, ptrlen; |
| char flg; |
| struct bpf_hdr *bpf; |
| struct ether_header *e; |
| |
| if (!eth || !data || !datlen) |
| return ANSH_FLG_ERR; |
| else |
| memset(data, 0, *datlen); |
| |
| 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; |
| if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) { |
| free(buf); |
| return ANSH_FLG_ERR; |
| } else { |
| pos += buflen; |
| *datlen = buflen; |
| ptr = next; |
| ptrlen = nextlen; |
| } |
| /* get additional packets from buffer */ |
| while (next && nextlen > 0) |
| if (_pkt_Recv(ptr, ptrlen, seq, 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 * |