--- ansh/src/utils.c 2011/10/13 11:01:37 1.1.1.1.2.5 +++ ansh/src/utils.c 2015/05/19 23:30:06 1.5.2.1 @@ -3,9 +3,46 @@ * by Michael Pounov * * $Author: misho $ - * $Id: utils.c,v 1.1.1.1.2.5 2011/10/13 11:01:37 misho Exp $ + * $Id: utils.c,v 1.5.2.1 2015/05/19 23:30:06 misho Exp $ * - *************************************************************************/ + ************************************************************************* +The ELWIX and AITNET software is distributed under the following +terms: + +All of the documentation and software included in the ELWIX and AITNET +Releases is copyrighted by ELWIX - Sofia/Bulgaria + +Copyright 2004 - 2015 + by Michael Pounov . All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +3. All advertising materials mentioning features or use of this software + must display the following acknowledgement: +This product includes software developed by Michael Pounov +ELWIX - Embedded LightWeight unIX and its contributors. +4. Neither the name of AITNET nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS +OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +SUCH DAMAGE. +*/ #include "global.h" @@ -60,6 +97,12 @@ PrepareL2(const char *psDev, int *bpflen) close(h); return -1; } + n = USHRT_MAX + 1; + if (ioctl(h, BIOCSBLEN, &n) == -1) { + printf("Error:: set buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno)); + close(h); + 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)); @@ -72,29 +115,27 @@ PrepareL2(const char *psDev, int *bpflen) return -1; } if (ioctl(h, BIOCGBLEN, bpflen) == -1) { - printf("Error:: get interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno)); + printf("Error:: get buffer 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; } int -PrepareL3(const struct sockaddr *sa, int *bpflen) +PrepareL3(const 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; @@ -106,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; @@ -121,8 +162,8 @@ PrepareL3(const struct sockaddr *sa, int *bpflen) } char -icmpRecv(int s, u_short * __restrict id, u_int * __restrict crypted, u_char * __restrict data, - int * __restrict datlen, struct sockaddr *sa, socklen_t *salen) +icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted, + u_char * __restrict data, int * __restrict datlen, sockaddr_t *sa, socklen_t *salen) { int ret = 0; struct icmp *icmp; @@ -130,7 +171,7 @@ icmpRecv(int s, u_short * __restrict id, u_int * __res 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; @@ -165,6 +206,8 @@ icmpRecv(int s, u_short * __restrict id, u_int * __res 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); } @@ -185,13 +228,16 @@ icmpRecv(int s, u_short * __restrict id, u_int * __res memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen); } + if (seq) + *seq = ntohl(hdr->ansh_seq); if (id) *id = ntohs(icmp->icmp_id); return hdr->ansh_flg; } 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, + sockaddr_t *sa, socklen_t salen) { u_char *pos, buf[USHRT_MAX] = { 0 }; struct icmp *icmp; @@ -212,6 +258,7 @@ icmpSend(int s, u_short id, char flg, u_int crypted, u hdr->ansh_flg = flg; hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr)); hdr->ansh_nonce = htonl(crypted); + hdr->ansh_seq = htonl(seq); hdr->ansh_crc = 0; hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); @@ -223,14 +270,14 @@ icmpSend(int s, u_short id, char flg, u_int crypted, u 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; } @@ -238,9 +285,9 @@ icmpSend(int s, u_short id, char flg, u_int crypted, u } static int -_pkt_Send(int s, 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, 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; @@ -256,10 +303,19 @@ _pkt_Send(int s, char flg, u_int crypted, u_char *data memcpy(pos, data, datlen); + if (Crypted) { + str = cryptBuffer(pos, datlen, Crypted); + if (str) { + memcpy(pos, str, datlen); + e_free(str); + } + } + 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_seq = htonl(seq); hdr->ansh_crc = 0; hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); @@ -270,7 +326,7 @@ _pkt_Send(int s, char flg, u_int crypted, u_char *data 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; } @@ -278,13 +334,13 @@ _pkt_Send(int s, char flg, u_int crypted, u_char *data } int -pktSend(int s, 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 e_ether_addr *ea) { int wlen, ret = 0; u_char *pos = data; while (datlen > -1) { - wlen = _pkt_Send(s, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea); + wlen = _pkt_Send(s, seq, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea); if (wlen == -1) return -1; else { @@ -298,7 +354,7 @@ pktSend(int s, char flg, u_int crypted, u_char *data, } static char -_pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict crypted, +_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) { @@ -306,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); @@ -338,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); } @@ -354,26 +413,40 @@ _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); + e_free(str); + } + } + memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen); } + if (seq) + *seq = ntohl(hdr->ansh_seq); return hdr->ansh_flg; } char -pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, - struct ether_header *eth) +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, *pos, *ptr; + u_char *buf, *next, *ptr, *pos = data; int nextlen, rlen, buflen, ptrlen; char flg; struct bpf_hdr *bpf; struct ether_header *e; - if (!eth || !datlen) + if (!eth || !data || !datlen) return ANSH_FLG_ERR; + else + memset(data, 0, *datlen); - if (!(buf = malloc(*datlen))) { + if (!(buf = e_malloc(*datlen))) { ERR("malloc() #%d - %s", errno, strerror(errno)); return ANSH_FLG_ERR; } @@ -381,7 +454,7 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __ rlen = read(s, buf, *datlen); if (rlen == -1) { ERR("Receive packet() #%d - %s", errno, strerror(errno)); - free(buf); + e_free(buf); return ANSH_FLG_ERR; } else VERB(4) LOG("Get packet with len=%d", rlen); @@ -389,7 +462,7 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __ /* 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); + e_free(buf); return ANSH_FLG_ERR; } else { bpf = (struct bpf_hdr*) buf; @@ -399,10 +472,8 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __ ptr = next = buf; ptrlen = nextlen = rlen; - pos = data; - buflen = *datlen; - if ((flg = _pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen)) == -1) { - free(buf); + if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) { + e_free(buf); return ANSH_FLG_ERR; } else { pos += buflen; @@ -410,8 +481,9 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __ ptr = next; ptrlen = nextlen; } + /* get additional packets from buffer */ while (next && nextlen > 0) - if (_pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen) == -1) + if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1) break; else { pos += buflen; @@ -420,7 +492,8 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __ ptrlen = nextlen; } - free(buf); + e_free(buf); + return flg; } @@ -457,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;