--- ansh/src/utils.c 2011/10/13 16:08:52 1.1.1.1.2.8 +++ ansh/src/utils.c 2012/07/22 22:41:33 1.4 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ - * $Id: utils.c,v 1.1.1.1.2.8 2011/10/13 16:08:52 misho Exp $ + * $Id: utils.c,v 1.4 2012/07/22 22:41:33 misho Exp $ * ************************************************************************* The ELWIX and AITNET software is distributed under the following @@ -12,7 +12,7 @@ terms: All of the documentation and software included in the ELWIX and AITNET Releases is copyrighted by ELWIX - Sofia/Bulgaria -Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011 +Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012 by Michael Pounov . All rights reserved. Redistribution and use in source and binary forms, with or without @@ -120,24 +120,22 @@ PrepareL2(const char *psDev, int *bpflen) 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; } int -PrepareL3(const struct sockaddr *sa, int *bpflen) +PrepareL3(const io_sockaddr_t *sa, int *bpflen) { int h, n = 1; FTRACE(3); assert(sa); - h = socket(sa->sa_family, SOCK_RAW, IPPROTO_ICMP); + h = socket(sa->sa.sa_family ? sa->sa.sa_family : AF_INET, SOCK_RAW, IPPROTO_ICMP); if (h == -1) { printf("Error:: Cant open raw socket #%d - %s\n", errno, strerror(errno)); return -1; @@ -149,7 +147,7 @@ PrepareL3(const struct sockaddr *sa, int *bpflen) return -1; } */ - if (bind(h, sa, sizeof(struct sockaddr)) == -1) { + if (sa->sa.sa_family && bind(h, &sa->sa, sa->sa.sa_len) == -1) { printf("Error:: Cant bind to raw socket #%d - %s\n", errno, strerror(errno)); close(h); return -1; @@ -165,7 +163,7 @@ PrepareL3(const struct sockaddr *sa, int *bpflen) char icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted, - u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen) + u_char * __restrict data, int * __restrict datlen, io_sockaddr_t *sa, socklen_t *salen) { int ret = 0; struct icmp *icmp; @@ -173,7 +171,7 @@ icmpRecv(int s, u_int * __restrict seq, u_short * __re u_char buf[USHRT_MAX] = { 0 }; u_int crc; - ret = recvfrom(s, buf, sizeof buf, 0, sa, salen); + ret = recvfrom(s, buf, sizeof buf, 0, &sa->sa, salen); if (ret == -1) { ERR("Receive recvfrom() #%d - %s", errno, strerror(errno)); return ANSH_FLG_ERR; @@ -208,6 +206,8 @@ icmpRecv(int s, u_int * __restrict seq, u_short * __re VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ..."); return ANSH_FLG_ERR; } + if (ntohl(hdr->ansh_nonce) != *crypted) + VERB(4) LOG("Detect change of nonce from %x to %x", *crypted, ntohl(hdr->ansh_nonce)); *crypted = ntohl(hdr->ansh_nonce); } @@ -237,7 +237,7 @@ icmpRecv(int s, u_int * __restrict seq, u_short * __re int 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) + io_sockaddr_t *sa, socklen_t salen) { u_char *pos, buf[USHRT_MAX] = { 0 }; struct icmp *icmp; @@ -270,14 +270,14 @@ icmpSend(int s, u_int seq, u_short id, char flg, u_int icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen); if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen, - 0, sa, salen)) == -1) { + 0, &sa->sa, salen)) == -1) { ERR("Send sendto() #%d - %s", errno, strerror(errno)); return ANSH_FLG_ERR; } else VERB(4) LOG("Put packet with len=%d", ret); if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) { VERB(3) LOG("Sended data %d is different from source data len %d", ret, - sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen); + (int) (sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen)); return ANSH_FLG_ERR; } @@ -285,9 +285,9 @@ icmpSend(int s, u_int seq, u_short id, char flg, u_int } static int -_pkt_Send(int s, u_int seq, 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, io_ether_addr_t *ea) { - u_char *pos, buf[USHRT_MAX] = { 0 }; + u_char *pos, *str, buf[USHRT_MAX] = { 0 }; struct ether_header *e = (struct ether_header*) buf; struct ansh_hdr *hdr; int ret = 0; @@ -297,12 +297,20 @@ _pkt_Send(int s, u_int seq, char flg, u_int crypted, u return ANSH_FLG_ERR; e->ether_type = ntohs(ANSH_ID); - memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN); + memcpy(e->ether_dhost, ea->ether_addr_octet, ETHER_ADDR_LEN); hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN); pos = ((u_char*) hdr) + sizeof(struct ansh_hdr); memcpy(pos, data, datlen); + if (Crypted) { + str = cryptBuffer(pos, datlen, Crypted); + if (str) { + memcpy(pos, str, datlen); + io_free(str); + } + } + hdr->ansh_ver = ANSH_VERSION; hdr->ansh_flg = flg; hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); @@ -318,7 +326,7 @@ _pkt_Send(int s, u_int seq, char flg, u_int crypted, u VERB(4) LOG("Put packet with len=%d", ret); if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) { VERB(3) LOG("Sended data %d is different from source data len %d", ret, - ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen); + (int) (ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)); return ANSH_FLG_ERR; } @@ -326,7 +334,7 @@ _pkt_Send(int s, u_int seq, char flg, u_int crypted, u } int -pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea) +pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *ea) { int wlen, ret = 0; u_char *pos = data; @@ -354,6 +362,7 @@ _pkt_Recv(u_char * __restrict buf, int rlen, u_int * _ struct bpf_hdr *bpf; struct ansh_hdr *hdr; u_int crc; + u_char *str; if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) { VERB(1) LOG("Discard packet too short %d ...", rlen); @@ -386,6 +395,8 @@ _pkt_Recv(u_char * __restrict buf, int rlen, u_int * _ VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ..."); return ANSH_FLG_ERR; } + if (ntohl(hdr->ansh_nonce) != *crypted) + VERB(4) LOG("Detect change of nonce from %x to %x", *crypted, ntohl(hdr->ansh_nonce)); *crypted = ntohl(hdr->ansh_nonce); } @@ -402,6 +413,16 @@ _pkt_Recv(u_char * __restrict buf, int rlen, u_int * _ /* select data */ if (data) { *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr); + if (Crypted) { + str = cryptBuffer(buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), + *datlen, Crypted); + if (str) { + memcpy(buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), + str, *datlen); + io_free(str); + } + } + memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen); } @@ -425,7 +446,7 @@ pktRecv(int s, u_int * __restrict seq, u_int * __restr else memset(data, 0, *datlen); - if (!(buf = malloc(*datlen))) { + if (!(buf = io_malloc(*datlen))) { ERR("malloc() #%d - %s", errno, strerror(errno)); return ANSH_FLG_ERR; } @@ -433,7 +454,7 @@ pktRecv(int s, u_int * __restrict seq, u_int * __restr rlen = read(s, buf, *datlen); if (rlen == -1) { ERR("Receive packet() #%d - %s", errno, strerror(errno)); - free(buf); + io_free(buf); return ANSH_FLG_ERR; } else VERB(4) LOG("Get packet with len=%d", rlen); @@ -441,7 +462,7 @@ pktRecv(int s, u_int * __restrict seq, u_int * __restr /* 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); + io_free(buf); return ANSH_FLG_ERR; } else { bpf = (struct bpf_hdr*) buf; @@ -452,7 +473,7 @@ pktRecv(int s, u_int * __restrict seq, u_int * __restr ptr = next = buf; ptrlen = nextlen = rlen; if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) { - free(buf); + io_free(buf); return ANSH_FLG_ERR; } else { pos += buflen; @@ -471,7 +492,7 @@ pktRecv(int s, u_int * __restrict seq, u_int * __restr ptrlen = nextlen; } - free(buf); + io_free(buf); return flg; } @@ -509,7 +530,7 @@ cryptBuffer(u_char *buf, int rlen, u_int ctr) 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) + if (io_ctr_AES(buf, rlen, &str, (u_char*) Key, ivec) == -1) return NULL; return str;