Annotation of ansh/src/utils.c, revision 1.1.1.1.2.9
1.1 misho 1: /*************************************************************************
2: * (C) 2011 AITNET - Sofia/Bulgaria - <office@aitnet.org>
3: * by Michael Pounov <misho@elwix.org>
4: *
5: * $Author: misho $
1.1.1.1.2.9! misho 6: * $Id: utils.c,v 1.1.1.1.2.8 2011/10/13 16:08:52 misho Exp $
1.1 misho 7: *
1.1.1.1.2.8 misho 8: *************************************************************************
9: The ELWIX and AITNET software is distributed under the following
10: terms:
11:
12: All of the documentation and software included in the ELWIX and AITNET
13: Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>
14:
15: Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011
16: by Michael Pounov <misho@elwix.org>. All rights reserved.
17:
18: Redistribution and use in source and binary forms, with or without
19: modification, are permitted provided that the following conditions
20: are met:
21: 1. Redistributions of source code must retain the above copyright
22: notice, this list of conditions and the following disclaimer.
23: 2. Redistributions in binary form must reproduce the above copyright
24: notice, this list of conditions and the following disclaimer in the
25: documentation and/or other materials provided with the distribution.
26: 3. All advertising materials mentioning features or use of this software
27: must display the following acknowledgement:
28: This product includes software developed by Michael Pounov <misho@elwix.org>
29: ELWIX - Embedded LightWeight unIX and its contributors.
30: 4. Neither the name of AITNET nor the names of its contributors
31: may be used to endorse or promote products derived from this software
32: without specific prior written permission.
33:
34: THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
35: ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
36: IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
37: ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
38: FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
39: DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
40: OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
41: HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
42: LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
43: OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
44: SUCH DAMAGE.
45: */
1.1 misho 46: #include "global.h"
47:
48:
49: void
50: Get1stEth(char *psDev, int devlen)
51: {
52: struct ifaddrs *ifa;
53:
54: assert(psDev);
55: assert(devlen > 0);
56:
57: getifaddrs(&ifa);
58: strlcpy(psDev, ifa->ifa_name, devlen);
59: freeifaddrs(ifa);
60: }
61:
62: int
63: PrepareL2(const char *psDev, int *bpflen)
64: {
65: int h, n = 1;
66: register int i;
67: char szStr[STRSIZ];
68: struct ifreq ifr;
1.1.1.1.2.3 misho 69: struct bpf_program fcode = { 0 };
70: struct bpf_insn insns[] = {
71: BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
72: BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ANSH_ID, 0, 1),
73: BPF_STMT(BPF_RET + BPF_K, -1),
74: BPF_STMT(BPF_RET + BPF_K, 0),
75: };
1.1 misho 76:
77: FTRACE(3);
78: assert(psDev);
79:
1.1.1.1.2.3 misho 80: fcode.bf_len = sizeof(insns) / sizeof(struct bpf_insn);
81: fcode.bf_insns = insns;
82:
1.1 misho 83: for (i = 0; i < 10; i++) {
84: memset(szStr, 0, sizeof szStr);
85: snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
86: h = open(szStr, O_RDWR);
87: if (h > 2)
88: break;
89: }
90: if (h < 3) {
91: printf("Error:: open bpf %s #%d - %s\n", szStr, errno, strerror(errno));
92: return -1;
93: }
94:
1.1.1.1.2.5 misho 95: if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {
1.1.1.1.2.3 misho 96: printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
97: close(h);
98: return -1;
99: }
1.1.1.1.2.7 misho 100: n = USHRT_MAX + 1;
101: if (ioctl(h, BIOCSBLEN, &n) == -1) {
102: printf("Error:: set buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
103: close(h);
104: return -1;
105: }
1.1.1.1.2.5 misho 106: strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);
107: if (ioctl(h, BIOCSETIF, &ifr) == -1) {
108: printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
1.1 misho 109: close(h);
110: return -1;
111: }
1.1.1.1.2.3 misho 112: if (ioctl(h, BIOCSETF, &fcode) == -1) {
113: printf("Error:: set filter interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
114: close(h);
115: return -1;
116: }
1.1 misho 117: if (ioctl(h, BIOCGBLEN, bpflen) == -1) {
1.1.1.1.2.7 misho 118: printf("Error:: get buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
1.1 misho 119: close(h);
120: return -1;
121: }
122:
1.1.1.1.2.4 misho 123: /*
1.1.1.1.2.3 misho 124: n = fcntl(h, F_GETFL);
125: fcntl(h, F_SETFL, n | O_NONBLOCK);
1.1.1.1.2.4 misho 126: */
1.1.1.1.2.3 misho 127:
1.1 misho 128: VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
129: return h;
130: }
131:
132: int
133: PrepareL3(const struct sockaddr *sa, int *bpflen)
134: {
135: int h, n = 1;
136:
137: FTRACE(3);
138: assert(sa);
139:
140: h = socket(sa->sa_family, SOCK_RAW, IPPROTO_ICMP);
141: if (h == -1) {
142: printf("Error:: Cant open raw socket #%d - %s\n", errno, strerror(errno));
143: return -1;
144: }
145: /*
146: if (setsockopt(h, SOL_SOCKET, SO_REUSEADDR, &n, sizeof n) == -1) {
147: printf("Error:: Cant set raw socket #%d - %s\n", errno, strerror(errno));
148: close(h);
149: return -1;
150: }
151: */
152: if (bind(h, sa, sizeof(struct sockaddr)) == -1) {
153: printf("Error:: Cant bind to raw socket #%d - %s\n", errno, strerror(errno));
154: close(h);
155: return -1;
156: }
157:
158: n = fcntl(h, F_GETFL);
159: fcntl(h, F_SETFL, n | O_NONBLOCK);
160:
161: *bpflen = USHRT_MAX;
162: VERB(3) LOG("Openned socket handle %d", h);
163: return h;
164: }
165:
166: char
1.1.1.1.2.6 misho 167: icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted,
168: u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
1.1 misho 169: {
170: int ret = 0;
171: struct icmp *icmp;
172: struct ansh_hdr *hdr;
173: u_char buf[USHRT_MAX] = { 0 };
174: u_int crc;
175:
176: ret = recvfrom(s, buf, sizeof buf, 0, sa, salen);
177: if (ret == -1) {
178: ERR("Receive recvfrom() #%d - %s", errno, strerror(errno));
179: return ANSH_FLG_ERR;
180: } else
181: VERB(4) LOG("Get packet with len=%d", ret);
182:
183: /* check header len */
184: if (ret < (sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr))) {
185: VERB(1) LOG("Discard packet too short %d ...", ret);
186: return ANSH_FLG_ERR;
187: } else
188: icmp = (struct icmp*) (buf + sizeof(struct ip));
189:
190: /* check echo magic ansh code */
191: if (icmp->icmp_type != ICMP_ECHOREPLY || icmp->icmp_code != ANSH_CODE) {
192: VERB(3) LOG("Packet isnt for me %d ... icmp_code=%d", ret, icmp->icmp_code);
193: return ANSH_FLG_ERR;
194: } else
195: hdr = (struct ansh_hdr*) (buf + sizeof(struct ip) + sizeof(struct icmp));
196:
197: /* check version and total size of packet */
198: if (hdr->ansh_ver != ANSH_VERSION) {
199: VERB(3) LOG("Packet with wrong version ...");
200: return ANSH_FLG_ERR;
201: }
1.1.1.1.2.1 misho 202: if (crypted) {
203: if (hdr->ansh_nonce && !*crypted) {
204: VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
205: return ANSH_FLG_ERR;
206: }
207: if (!hdr->ansh_nonce && *crypted) {
208: VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
209: return ANSH_FLG_ERR;
210: }
211:
212: *crypted = ntohl(hdr->ansh_nonce);
213: }
1.1 misho 214:
215: /* check crc of packet */
216: crc = hdr->ansh_crc;
217: hdr->ansh_crc ^= hdr->ansh_crc;
218: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
219: if (crc != hdr->ansh_crc) {
220: VERB(3) LOG("Packet with wrong crc ...");
221: return ANSH_FLG_ERR;
222: }
223:
224: /* copy data */
225: if (data && datlen) {
226: memset(data, 0, *datlen);
227: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
228: memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
229: }
230:
1.1.1.1.2.6 misho 231: if (seq)
232: *seq = ntohl(hdr->ansh_seq);
1.1 misho 233: if (id)
234: *id = ntohs(icmp->icmp_id);
235: return hdr->ansh_flg;
236: }
237:
238: int
1.1.1.1.2.6 misho 239: icmpSend(int s, u_int seq, u_short id, char flg, u_int crypted, u_char *data, int datlen,
240: struct sockaddr *sa, socklen_t salen)
1.1 misho 241: {
242: u_char *pos, buf[USHRT_MAX] = { 0 };
243: struct icmp *icmp;
244: struct ansh_hdr *hdr;
245: int ret = 0;
246:
247: assert(data);
248: if ((sizeof buf - sizeof(struct icmp) + sizeof(struct ansh_hdr)) < datlen)
249: return ANSH_FLG_ERR;
250:
251: icmp = (struct icmp*) buf;
252: hdr = (struct ansh_hdr*) (buf + sizeof(struct icmp));
253: pos = buf + sizeof(struct icmp) + sizeof(struct ansh_hdr);
254:
255: memcpy(pos, data, datlen);
256:
257: hdr->ansh_ver = ANSH_VERSION;
258: hdr->ansh_flg = flg;
259: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
1.1.1.1.2.1 misho 260: hdr->ansh_nonce = htonl(crypted);
1.1.1.1.2.6 misho 261: hdr->ansh_seq = htonl(seq);
1.1 misho 262: hdr->ansh_crc = 0;
263: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
264:
265: icmp->icmp_type = ICMP_ECHOREPLY;
266: icmp->icmp_code = ANSH_CODE;
267: icmp->icmp_id = htons(id);
268: icmp->icmp_seq = htons(datlen);
269: icmp->icmp_cksum = 0;
270: icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
271:
272: if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen,
273: 0, sa, salen)) == -1) {
274: ERR("Send sendto() #%d - %s", errno, strerror(errno));
275: return ANSH_FLG_ERR;
276: } else
277: VERB(4) LOG("Put packet with len=%d", ret);
278: if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
279: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
1.1.1.1.2.9! misho 280: (int) (sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen));
1.1 misho 281: return ANSH_FLG_ERR;
282: }
283:
284: return ret;
285: }
286:
1.1.1.1.2.5 misho 287: static int
1.1.1.1.2.9! misho 288: _pkt_Send(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *ea)
1.1 misho 289: {
290: u_char *pos, buf[USHRT_MAX] = { 0 };
291: struct ether_header *e = (struct ether_header*) buf;
292: struct ansh_hdr *hdr;
293: int ret = 0;
294:
295: assert(data);
296: if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
297: return ANSH_FLG_ERR;
298:
1.1.1.1.2.3 misho 299: e->ether_type = ntohs(ANSH_ID);
1.1.1.1.2.9! misho 300: memcpy(e->ether_dhost, ea->ether_addr_octet, ETHER_ADDR_LEN);
1.1 misho 301: hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
302: pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
303:
304: memcpy(pos, data, datlen);
305:
306: hdr->ansh_ver = ANSH_VERSION;
307: hdr->ansh_flg = flg;
308: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
1.1.1.1.2.2 misho 309: hdr->ansh_nonce = htonl(crypted);
1.1.1.1.2.6 misho 310: hdr->ansh_seq = htonl(seq);
1.1 misho 311: hdr->ansh_crc = 0;
312: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
313:
314: if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
315: ERR("Send packet() #%d - %s", errno, strerror(errno));
316: return ANSH_FLG_ERR;
317: } else
318: VERB(4) LOG("Put packet with len=%d", ret);
319: if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
320: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
1.1.1.1.2.9! misho 321: (int) (ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen));
1.1 misho 322: return ANSH_FLG_ERR;
323: }
324:
325: return ret;
326: }
327:
1.1.1.1.2.5 misho 328: int
1.1.1.1.2.9! misho 329: pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *ea)
1.1 misho 330: {
1.1.1.1.2.5 misho 331: int wlen, ret = 0;
332: u_char *pos = data;
1.1 misho 333:
1.1.1.1.2.5 misho 334: while (datlen > -1) {
1.1.1.1.2.6 misho 335: wlen = _pkt_Send(s, seq, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
1.1.1.1.2.5 misho 336: if (wlen == -1)
337: return -1;
338: else {
339: pos += wlen;
340: datlen -= wlen;
341: ret += wlen;
342: }
1.1 misho 343: }
344:
1.1.1.1.2.5 misho 345: return ret;
346: }
1.1 misho 347:
1.1.1.1.2.5 misho 348: static char
1.1.1.1.2.6 misho 349: _pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict seq, u_int * __restrict crypted,
1.1.1.1.2.5 misho 350: u_char * __restrict data, int * __restrict datlen,
351: u_char ** __restrict next, int * __restrict nextlen)
352: {
353: int bias;
354: struct bpf_hdr *bpf;
355: struct ansh_hdr *hdr;
356: u_int crc;
357:
358: if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
359: VERB(1) LOG("Discard packet too short %d ...", rlen);
1.1 misho 360: return ANSH_FLG_ERR;
361: } else {
362: bpf = (struct bpf_hdr*) buf;
363: hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
364: }
365:
1.1.1.1.2.5 misho 366: /* slice readed data to packets */
367: if ((bias = BPF_WORDALIGN(bpf->bh_hdrlen + bpf->bh_caplen)) < rlen) {
368: *next = buf + bias;
369: *nextlen = rlen - bias;
370: } else {
371: *next = NULL;
372: *nextlen = 0;
373: }
374:
1.1 misho 375: /* check version and total size of packet */
376: if (hdr->ansh_ver != ANSH_VERSION) {
377: VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
378: return ANSH_FLG_ERR;
379: }
1.1.1.1.2.2 misho 380: if (crypted) {
381: if (hdr->ansh_nonce && !*crypted) {
382: VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
383: return ANSH_FLG_ERR;
384: }
385: if (!hdr->ansh_nonce && *crypted) {
386: VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
387: return ANSH_FLG_ERR;
388: }
389:
390: *crypted = ntohl(hdr->ansh_nonce);
391: }
392:
1.1 misho 393: /* check crc of packet */
394: crc = hdr->ansh_crc;
395: hdr->ansh_crc ^= hdr->ansh_crc;
396: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
397: if (crc != hdr->ansh_crc) {
398: VERB(3) LOG("Packet with wrong crc ...");
399: return ANSH_FLG_ERR;
400: }
401:
1.1.1.1.2.5 misho 402: /* select data */
1.1 misho 403: if (data) {
404: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
1.1.1.1.2.4 misho 405: memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen);
1.1 misho 406: }
407:
1.1.1.1.2.6 misho 408: if (seq)
409: *seq = ntohl(hdr->ansh_seq);
1.1.1.1.2.5 misho 410: return hdr->ansh_flg;
411: }
412:
413: char
1.1.1.1.2.6 misho 414: pktRecv(int s, u_int * __restrict seq, u_int * __restrict crypted, u_char * __restrict data,
415: int * __restrict datlen, struct ether_header *eth)
1.1.1.1.2.5 misho 416: {
1.1.1.1.2.6 misho 417: u_char *buf, *next, *ptr, *pos = data;
1.1.1.1.2.5 misho 418: int nextlen, rlen, buflen, ptrlen;
419: char flg;
420: struct bpf_hdr *bpf;
421: struct ether_header *e;
422:
1.1.1.1.2.6 misho 423: if (!eth || !data || !datlen)
1.1.1.1.2.5 misho 424: return ANSH_FLG_ERR;
1.1.1.1.2.6 misho 425: else
426: memset(data, 0, *datlen);
1.1.1.1.2.5 misho 427:
428: if (!(buf = malloc(*datlen))) {
429: ERR("malloc() #%d - %s", errno, strerror(errno));
430: return ANSH_FLG_ERR;
431: }
432:
433: rlen = read(s, buf, *datlen);
434: if (rlen == -1) {
435: ERR("Receive packet() #%d - %s", errno, strerror(errno));
436: free(buf);
437: return ANSH_FLG_ERR;
438: } else
439: VERB(4) LOG("Get packet with len=%d", rlen);
440:
441: /* check header len */
442: if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
443: VERB(1) LOG("Discard packet too short %d ...", rlen);
444: free(buf);
445: return ANSH_FLG_ERR;
446: } else {
447: bpf = (struct bpf_hdr*) buf;
448: e = (struct ether_header*) (buf + bpf->bh_hdrlen);
449: memcpy(eth, e, ETHER_HDR_LEN);
450: }
451:
452: ptr = next = buf;
453: ptrlen = nextlen = rlen;
1.1.1.1.2.6 misho 454: if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) {
1.1.1.1.2.5 misho 455: free(buf);
456: return ANSH_FLG_ERR;
457: } else {
458: pos += buflen;
459: *datlen = buflen;
460: ptr = next;
461: ptrlen = nextlen;
462: }
1.1.1.1.2.6 misho 463: /* get additional packets from buffer */
1.1.1.1.2.5 misho 464: while (next && nextlen > 0)
1.1.1.1.2.6 misho 465: if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1)
1.1.1.1.2.5 misho 466: break;
467: else {
468: pos += buflen;
469: *datlen += buflen;
470: ptr = next;
471: ptrlen = nextlen;
472: }
473:
1.1 misho 474: free(buf);
1.1.1.1.2.6 misho 475:
1.1.1.1.2.5 misho 476: return flg;
1.1 misho 477: }
478:
479: void *
480: TOfunc(sched_task_t *task)
481: {
482: struct tagProc *proc;
483:
484: FTRACE(3);
485:
486: /* not found argument, drop data */
487: if (!(proc = TASK_ARG(task)))
488: return (void*) -1;
489:
490: if (proc->proc_pid)
491: kill(proc->proc_pid, SIGTERM);
492:
493: return NULL;
494: }
495:
1.1.1.1.2.1 misho 496: u_char *
497: cryptBuffer(u_char *buf, int rlen, u_int ctr)
498: {
499: u_char *str, ivec[AES_BLOCK_SIZE] = { 0 };
500: u_int rctr = htonl(ctr);
501:
502: FTRACE(3);
503:
504: if (!buf)
505: return NULL;
506:
507: memcpy(ivec, &ctr, sizeof ctr);
508: memcpy(ivec + 4, &rctr, sizeof rctr);
509: memcpy(ivec + 8, &ctr, sizeof ctr);
510: memcpy(ivec + 12, &rctr, sizeof rctr);
511:
512: if (io_ctr_AES(buf, rlen, &str, (u_char*) "_ansh_ELWIX_", ivec) == -1)
513: return NULL;
514:
515: return str;
516: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>
![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to utils.c CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [ELWIX - Embedded LightWeight unIX -] / ansh / src