--- ansh/src/utils.c 2011/10/04 22:37:46 1.1.1.1 +++ ansh/src/utils.c 2011/10/10 14:15:40 1.1.1.1.2.4 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ - * $Id: utils.c,v 1.1.1.1 2011/10/04 22:37:46 misho Exp $ + * $Id: utils.c,v 1.1.1.1.2.4 2011/10/10 14:15:40 misho Exp $ * *************************************************************************/ #include "global.h" @@ -29,10 +29,20 @@ PrepareL2(const char *psDev, int *bpflen) register int i; char szStr[STRSIZ]; 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); assert(psDev); + fcode.bf_len = sizeof(insns) / sizeof(struct bpf_insn); + fcode.bf_insns = insns; + for (i = 0; i < 10; i++) { memset(szStr, 0, sizeof szStr); snprintf(szStr, sizeof szStr, "/dev/bpf%d", i); @@ -51,17 +61,34 @@ PrepareL2(const char *psDev, int *bpflen) close(h); return -1; } + /* + if (ioctl(h, BIOCSHDRCMPLT, &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)); close(h); return -1; } + if (ioctl(h, BIOCSETF, &fcode) == -1) { + printf("Error:: set filter interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno)); + close(h); + return -1; + } if (ioctl(h, BIOCGBLEN, bpflen) == -1) { printf("Error:: get interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno)); close(h); 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); return h; } @@ -101,7 +128,7 @@ PrepareL3(const struct sockaddr *sa, int *bpflen) } char -icmpRecv(int s, u_short * __restrict id, u_char * __restrict data, +icmpRecv(int s, u_short * __restrict id, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen) { int ret = 0; @@ -136,7 +163,19 @@ icmpRecv(int s, u_short * __restrict id, u_char * __re VERB(3) LOG("Packet with wrong version ..."); return ANSH_FLG_ERR; } + if (crypted) { + if (hdr->ansh_nonce && !*crypted) { + VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ..."); + return ANSH_FLG_ERR; + } + if (!hdr->ansh_nonce && *crypted) { + VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ..."); + return ANSH_FLG_ERR; + } + *crypted = ntohl(hdr->ansh_nonce); + } + /* check crc of packet */ crc = hdr->ansh_crc; hdr->ansh_crc ^= hdr->ansh_crc; @@ -159,7 +198,7 @@ icmpRecv(int s, u_short * __restrict id, u_char * __re } int -icmpSend(int s, u_short id, char flg, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen) +icmpSend(int s, 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 }; struct icmp *icmp; @@ -179,6 +218,7 @@ icmpSend(int s, u_short id, char flg, u_char *data, in hdr->ansh_ver = ANSH_VERSION; hdr->ansh_flg = flg; hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); + hdr->ansh_nonce = htonl(crypted); hdr->ansh_crc = 0; hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); @@ -205,7 +245,7 @@ icmpSend(int s, u_short id, char flg, u_char *data, in } int -pktSend(int s, u_short id, char flg, u_char *data, int datlen, struct ether_addr *ea) +pktSend(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; @@ -216,7 +256,7 @@ pktSend(int s, u_short id, char flg, u_char *data, int if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen) return ANSH_FLG_ERR; - e->ether_type = htons(id); + e->ether_type = ntohs(ANSH_ID); memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN); hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN); pos = ((u_char*) hdr) + sizeof(struct ansh_hdr); @@ -226,6 +266,7 @@ pktSend(int s, u_short id, char flg, u_char *data, int hdr->ansh_ver = ANSH_VERSION; hdr->ansh_flg = flg; hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); + hdr->ansh_nonce = htonl(crypted); hdr->ansh_crc = 0; hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); @@ -244,7 +285,8 @@ pktSend(int s, u_short id, char flg, u_char *data, int } char -pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth) +pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, + struct ether_header *eth) { int ret = 0; struct bpf_hdr *bpf; @@ -287,6 +329,19 @@ pktRecv(int s, u_char * __restrict data, int * __restr free(buf); return ANSH_FLG_ERR; } + if (crypted) { + if (hdr->ansh_nonce && !*crypted) { + VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ..."); + return ANSH_FLG_ERR; + } + if (!hdr->ansh_nonce && *crypted) { + VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ..."); + return ANSH_FLG_ERR; + } + + *crypted = ntohl(hdr->ansh_nonce); + } + /* check crc of packet */ crc = hdr->ansh_crc; hdr->ansh_crc ^= hdr->ansh_crc; @@ -301,7 +356,7 @@ pktRecv(int s, u_char * __restrict data, int * __restr if (data) { memset(data, 0, *datlen); *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; @@ -326,3 +381,24 @@ TOfunc(sched_task_t *task) return NULL; } +u_char * +cryptBuffer(u_char *buf, int rlen, u_int ctr) +{ + u_char *str, ivec[AES_BLOCK_SIZE] = { 0 }; + u_int rctr = htonl(ctr); + + FTRACE(3); + + if (!buf) + return NULL; + + memcpy(ivec, &ctr, sizeof ctr); + memcpy(ivec + 4, &rctr, sizeof rctr); + memcpy(ivec + 8, &ctr, sizeof ctr); + memcpy(ivec + 12, &rctr, sizeof rctr); + + if (io_ctr_AES(buf, rlen, &str, (u_char*) "_ansh_ELWIX_", ivec) == -1) + return NULL; + + return str; +}