--- ansh/src/utils.c 2011/10/04 22:37:46 1.1 +++ ansh/src/utils.c 2011/10/17 20:14:02 1.2 @@ -3,9 +3,46 @@ * by Michael Pounov * * $Author: misho $ - * $Id: utils.c,v 1.1 2011/10/04 22:37:46 misho Exp $ + * $Id: utils.c,v 1.2 2011/10/17 20:14:02 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, 2005, 2006, 2007, 2008, 2009, 2010, 2011 + 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" @@ -29,10 +66,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); @@ -45,23 +92,37 @@ PrepareL2(const char *psDev, int *bpflen) 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; + } + 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)); close(h); return -1; } - if (ioctl(h, BIOCIMMEDIATE, &n) == -1) { - printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno)); + 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)); + 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; } @@ -101,8 +162,8 @@ PrepareL3(const struct sockaddr *sa, int *bpflen) } char -icmpRecv(int s, u_short * __restrict id, 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, struct sockaddr *sa, socklen_t *salen) { int ret = 0; struct icmp *icmp; @@ -136,7 +197,21 @@ 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; + } + 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); + } + /* check crc of packet */ crc = hdr->ansh_crc; hdr->ansh_crc ^= hdr->ansh_crc; @@ -153,13 +228,16 @@ icmpRecv(int s, u_short * __restrict id, u_char * __re 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_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 }; struct icmp *icmp; @@ -179,6 +257,8 @@ 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_seq = htonl(seq); hdr->ansh_crc = 0; hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); @@ -197,17 +277,17 @@ icmpSend(int s, u_short id, char flg, u_char *data, in 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; } return ret; } -int -pktSend(int s, u_short id, char flg, u_char *data, int datlen, struct ether_addr *ea) +static int +_pkt_Send(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *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; @@ -216,16 +296,26 @@ 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); - memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN); + e->ether_type = ntohs(ANSH_ID); + 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); + 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))); @@ -236,77 +326,175 @@ pktSend(int s, u_short id, char flg, u_char *data, int 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; } return ret; } -char -pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth) +int +pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *ea) { - 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 ether_header *e; struct ansh_hdr *hdr; - u_char *buf; u_int crc; + u_char *str; - 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; + } else { + bpf = (struct bpf_hdr*) buf; + 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); + 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; + } + 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); + } + + /* check crc of packet */ + crc = hdr->ansh_crc; + hdr->ansh_crc ^= hdr->ansh_crc; + hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len))); + if (crc != hdr->ansh_crc) { + VERB(3) LOG("Packet with wrong crc ..."); + return ANSH_FLG_ERR; + } + + /* 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); + 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 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; } - ret = read(s, buf, *datlen); - if (ret == -1) { + 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", ret); + VERB(4) LOG("Get packet with len=%d", rlen); /* 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); + 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); - hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN); } - /* check version and total size of packet */ - if (hdr->ansh_ver != ANSH_VERSION) { - VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver); + 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; } - /* check crc of packet */ - crc = hdr->ansh_crc; - hdr->ansh_crc ^= hdr->ansh_crc; - 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; - } + /* 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; + } - /* copy data */ - if (data) { - memset(data, 0, *datlen); - *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr); - memcpy(data, hdr + sizeof(struct ansh_hdr), *datlen); - } - - ret = (char) hdr->ansh_flg; free(buf); - return (char) ret; + + return flg; } void * @@ -326,3 +514,53 @@ 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*) Key, ivec) == -1) + return NULL; + + return str; +} + +int +stopProcess(sched_root_task_t * __restrict root, proc_head_t * __restrict h, pid_t pid, sched_task_func_t func) +{ + struct tagProc *p; + + FTRACE(3); + + SLIST_FOREACH(p, h, proc_next) + if (p->proc_pid == pid) { + break; + } + VERB(3) LOG("pid=%d found=%p\n", pid, p); + if (!p) + return 1; + + ioFreePTY(p->proc_pty, p->proc_ttyname); + if (p->proc_pty) + schedCancelby(root, NULL, CRITERIA_FD, (void*) ((intptr_t) p->proc_pty), NULL); + + p->proc_pty = 0; + p->proc_pid = 0; + p->proc_seq = 0; + p->proc_flg = ANSH_FLG_EOF; + p->proc_rlen_[FD2NET] = 0; + + schedCallOnce(root, func, p, p->proc_sock); + return 0; +}