1: /*************************************************************************
2: * (C) 2011 AITNET - Sofia/Bulgaria - <office@aitnet.org>
3: * by Michael Pounov <misho@elwix.org>
4: *
5: * $Author: misho $
6: * $Id: utils.c,v 1.2.2.1 2011/10/31 09:46:18 misho Exp $
7: *
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: */
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;
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: };
76:
77: FTRACE(3);
78: assert(psDev);
79:
80: fcode.bf_len = sizeof(insns) / sizeof(struct bpf_insn);
81: fcode.bf_insns = insns;
82:
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:
95: if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {
96: printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
97: close(h);
98: return -1;
99: }
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: }
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));
109: close(h);
110: return -1;
111: }
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: }
117: if (ioctl(h, BIOCGBLEN, bpflen) == -1) {
118: printf("Error:: get buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
119: close(h);
120: return -1;
121: }
122:
123: n = fcntl(h, F_GETFL);
124: fcntl(h, F_SETFL, n | O_NONBLOCK);
125:
126: VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
127: return h;
128: }
129:
130: int
131: PrepareL3(const struct sockaddr *sa, int *bpflen)
132: {
133: int h, n = 1;
134:
135: FTRACE(3);
136:
137: h = socket(sa->sa_family, SOCK_RAW, IPPROTO_ICMP);
138: if (h == -1) {
139: printf("Error:: Cant open raw socket #%d - %s\n", errno, strerror(errno));
140: return -1;
141: }
142: /*
143: if (setsockopt(h, SOL_SOCKET, SO_REUSEADDR, &n, sizeof n) == -1) {
144: printf("Error:: Cant set raw socket #%d - %s\n", errno, strerror(errno));
145: close(h);
146: return -1;
147: }
148: */
149: if (sa && bind(h, sa, sizeof(struct sockaddr)) == -1) {
150: printf("Error:: Cant bind to raw socket #%d - %s\n", errno, strerror(errno));
151: close(h);
152: return -1;
153: }
154:
155: n = fcntl(h, F_GETFL);
156: fcntl(h, F_SETFL, n | O_NONBLOCK);
157:
158: *bpflen = USHRT_MAX;
159: VERB(3) LOG("Openned socket handle %d", h);
160: return h;
161: }
162:
163: char
164: icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted,
165: u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
166: {
167: int ret = 0;
168: struct icmp *icmp;
169: struct ansh_hdr *hdr;
170: u_char buf[USHRT_MAX] = { 0 };
171: u_int crc;
172:
173: ret = recvfrom(s, buf, sizeof buf, 0, sa, salen);
174: if (ret == -1) {
175: ERR("Receive recvfrom() #%d - %s", errno, strerror(errno));
176: return ANSH_FLG_ERR;
177: } else
178: VERB(4) LOG("Get packet with len=%d", ret);
179:
180: /* check header len */
181: if (ret < (sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr))) {
182: VERB(1) LOG("Discard packet too short %d ...", ret);
183: return ANSH_FLG_ERR;
184: } else
185: icmp = (struct icmp*) (buf + sizeof(struct ip));
186:
187: /* check echo magic ansh code */
188: if (icmp->icmp_type != ICMP_ECHOREPLY || icmp->icmp_code != ANSH_CODE) {
189: VERB(3) LOG("Packet isnt for me %d ... icmp_code=%d", ret, icmp->icmp_code);
190: return ANSH_FLG_ERR;
191: } else
192: hdr = (struct ansh_hdr*) (buf + sizeof(struct ip) + sizeof(struct icmp));
193:
194: /* check version and total size of packet */
195: if (hdr->ansh_ver != ANSH_VERSION) {
196: VERB(3) LOG("Packet with wrong version ...");
197: return ANSH_FLG_ERR;
198: }
199: if (crypted) {
200: if (hdr->ansh_nonce && !*crypted) {
201: VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
202: return ANSH_FLG_ERR;
203: }
204: if (!hdr->ansh_nonce && *crypted) {
205: VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
206: return ANSH_FLG_ERR;
207: }
208: if (ntohl(hdr->ansh_nonce) != *crypted)
209: VERB(4) LOG("Detect change of nonce from %x to %x", *crypted, ntohl(hdr->ansh_nonce));
210:
211: *crypted = ntohl(hdr->ansh_nonce);
212: }
213:
214: /* check crc of packet */
215: crc = hdr->ansh_crc;
216: hdr->ansh_crc ^= hdr->ansh_crc;
217: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
218: if (crc != hdr->ansh_crc) {
219: VERB(3) LOG("Packet with wrong crc ...");
220: return ANSH_FLG_ERR;
221: }
222:
223: /* copy data */
224: if (data && datlen) {
225: memset(data, 0, *datlen);
226: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
227: memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
228: }
229:
230: if (seq)
231: *seq = ntohl(hdr->ansh_seq);
232: if (id)
233: *id = ntohs(icmp->icmp_id);
234: return hdr->ansh_flg;
235: }
236:
237: int
238: icmpSend(int s, u_int seq, u_short id, char flg, u_int crypted, u_char *data, int datlen,
239: struct sockaddr *sa, socklen_t salen)
240: {
241: u_char *pos, buf[USHRT_MAX] = { 0 };
242: struct icmp *icmp;
243: struct ansh_hdr *hdr;
244: int ret = 0;
245:
246: assert(data);
247: if ((sizeof buf - sizeof(struct icmp) + sizeof(struct ansh_hdr)) < datlen)
248: return ANSH_FLG_ERR;
249:
250: icmp = (struct icmp*) buf;
251: hdr = (struct ansh_hdr*) (buf + sizeof(struct icmp));
252: pos = buf + sizeof(struct icmp) + sizeof(struct ansh_hdr);
253:
254: memcpy(pos, data, datlen);
255:
256: hdr->ansh_ver = ANSH_VERSION;
257: hdr->ansh_flg = flg;
258: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
259: hdr->ansh_nonce = htonl(crypted);
260: hdr->ansh_seq = htonl(seq);
261: hdr->ansh_crc = 0;
262: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
263:
264: icmp->icmp_type = ICMP_ECHOREPLY;
265: icmp->icmp_code = ANSH_CODE;
266: icmp->icmp_id = htons(id);
267: icmp->icmp_seq = htons(datlen);
268: icmp->icmp_cksum = 0;
269: icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
270:
271: if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen,
272: 0, sa, salen)) == -1) {
273: ERR("Send sendto() #%d - %s", errno, strerror(errno));
274: return ANSH_FLG_ERR;
275: } else
276: VERB(4) LOG("Put packet with len=%d", ret);
277: if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
278: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
279: (int) (sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen));
280: return ANSH_FLG_ERR;
281: }
282:
283: return ret;
284: }
285:
286: static int
287: _pkt_Send(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *ea)
288: {
289: u_char *pos, *str, buf[USHRT_MAX] = { 0 };
290: struct ether_header *e = (struct ether_header*) buf;
291: struct ansh_hdr *hdr;
292: int ret = 0;
293:
294: assert(data);
295: if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
296: return ANSH_FLG_ERR;
297:
298: e->ether_type = ntohs(ANSH_ID);
299: memcpy(e->ether_dhost, ea->ether_addr_octet, ETHER_ADDR_LEN);
300: hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
301: pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
302:
303: memcpy(pos, data, datlen);
304:
305: if (Crypted) {
306: str = cryptBuffer(pos, datlen, Crypted);
307: if (str) {
308: memcpy(pos, str, datlen);
309: free(str);
310: }
311: }
312:
313: hdr->ansh_ver = ANSH_VERSION;
314: hdr->ansh_flg = flg;
315: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
316: hdr->ansh_nonce = htonl(crypted);
317: hdr->ansh_seq = htonl(seq);
318: hdr->ansh_crc = 0;
319: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
320:
321: if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
322: ERR("Send packet() #%d - %s", errno, strerror(errno));
323: return ANSH_FLG_ERR;
324: } else
325: VERB(4) LOG("Put packet with len=%d", ret);
326: if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
327: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
328: (int) (ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen));
329: return ANSH_FLG_ERR;
330: }
331:
332: return ret;
333: }
334:
335: int
336: pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct io_ether_addr *ea)
337: {
338: int wlen, ret = 0;
339: u_char *pos = data;
340:
341: while (datlen > -1) {
342: wlen = _pkt_Send(s, seq, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
343: if (wlen == -1)
344: return -1;
345: else {
346: pos += wlen;
347: datlen -= wlen;
348: ret += wlen;
349: }
350: }
351:
352: return ret;
353: }
354:
355: static char
356: _pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict seq, u_int * __restrict crypted,
357: u_char * __restrict data, int * __restrict datlen,
358: u_char ** __restrict next, int * __restrict nextlen)
359: {
360: int bias;
361: struct bpf_hdr *bpf;
362: struct ansh_hdr *hdr;
363: u_int crc;
364: u_char *str;
365:
366: if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
367: VERB(1) LOG("Discard packet too short %d ...", rlen);
368: return ANSH_FLG_ERR;
369: } else {
370: bpf = (struct bpf_hdr*) buf;
371: hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
372: }
373:
374: /* slice readed data to packets */
375: if ((bias = BPF_WORDALIGN(bpf->bh_hdrlen + bpf->bh_caplen)) < rlen) {
376: *next = buf + bias;
377: *nextlen = rlen - bias;
378: } else {
379: *next = NULL;
380: *nextlen = 0;
381: }
382:
383: /* check version and total size of packet */
384: if (hdr->ansh_ver != ANSH_VERSION) {
385: VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
386: return ANSH_FLG_ERR;
387: }
388: if (crypted) {
389: if (hdr->ansh_nonce && !*crypted) {
390: VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
391: return ANSH_FLG_ERR;
392: }
393: if (!hdr->ansh_nonce && *crypted) {
394: VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
395: return ANSH_FLG_ERR;
396: }
397: if (ntohl(hdr->ansh_nonce) != *crypted)
398: VERB(4) LOG("Detect change of nonce from %x to %x", *crypted, ntohl(hdr->ansh_nonce));
399:
400: *crypted = ntohl(hdr->ansh_nonce);
401: }
402:
403: /* check crc of packet */
404: crc = hdr->ansh_crc;
405: hdr->ansh_crc ^= hdr->ansh_crc;
406: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
407: if (crc != hdr->ansh_crc) {
408: VERB(3) LOG("Packet with wrong crc ...");
409: return ANSH_FLG_ERR;
410: }
411:
412: /* select data */
413: if (data) {
414: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
415: if (Crypted) {
416: str = cryptBuffer(buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr),
417: *datlen, Crypted);
418: if (str) {
419: memcpy(buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr),
420: str, *datlen);
421: free(str);
422: }
423: }
424:
425: memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen);
426: }
427:
428: if (seq)
429: *seq = ntohl(hdr->ansh_seq);
430: return hdr->ansh_flg;
431: }
432:
433: char
434: pktRecv(int s, u_int * __restrict seq, u_int * __restrict crypted, u_char * __restrict data,
435: int * __restrict datlen, struct ether_header *eth)
436: {
437: u_char *buf, *next, *ptr, *pos = data;
438: int nextlen, rlen, buflen, ptrlen;
439: char flg;
440: struct bpf_hdr *bpf;
441: struct ether_header *e;
442:
443: if (!eth || !data || !datlen)
444: return ANSH_FLG_ERR;
445: else
446: memset(data, 0, *datlen);
447:
448: if (!(buf = malloc(*datlen))) {
449: ERR("malloc() #%d - %s", errno, strerror(errno));
450: return ANSH_FLG_ERR;
451: }
452:
453: rlen = read(s, buf, *datlen);
454: if (rlen == -1) {
455: ERR("Receive packet() #%d - %s", errno, strerror(errno));
456: free(buf);
457: return ANSH_FLG_ERR;
458: } else
459: VERB(4) LOG("Get packet with len=%d", rlen);
460:
461: /* check header len */
462: if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
463: VERB(1) LOG("Discard packet too short %d ...", rlen);
464: free(buf);
465: return ANSH_FLG_ERR;
466: } else {
467: bpf = (struct bpf_hdr*) buf;
468: e = (struct ether_header*) (buf + bpf->bh_hdrlen);
469: memcpy(eth, e, ETHER_HDR_LEN);
470: }
471:
472: ptr = next = buf;
473: ptrlen = nextlen = rlen;
474: if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) {
475: free(buf);
476: return ANSH_FLG_ERR;
477: } else {
478: pos += buflen;
479: *datlen = buflen;
480: ptr = next;
481: ptrlen = nextlen;
482: }
483: /* get additional packets from buffer */
484: while (next && nextlen > 0)
485: if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1)
486: break;
487: else {
488: pos += buflen;
489: *datlen += buflen;
490: ptr = next;
491: ptrlen = nextlen;
492: }
493:
494: free(buf);
495:
496: return flg;
497: }
498:
499: void *
500: TOfunc(sched_task_t *task)
501: {
502: struct tagProc *proc;
503:
504: FTRACE(3);
505:
506: /* not found argument, drop data */
507: if (!(proc = TASK_ARG(task)))
508: return (void*) -1;
509:
510: if (proc->proc_pid)
511: kill(proc->proc_pid, SIGTERM);
512:
513: return NULL;
514: }
515:
516: u_char *
517: cryptBuffer(u_char *buf, int rlen, u_int ctr)
518: {
519: u_char *str, ivec[AES_BLOCK_SIZE] = { 0 };
520: u_int rctr = htonl(ctr);
521:
522: FTRACE(3);
523:
524: if (!buf)
525: return NULL;
526:
527: memcpy(ivec, &ctr, sizeof ctr);
528: memcpy(ivec + 4, &rctr, sizeof rctr);
529: memcpy(ivec + 8, &ctr, sizeof ctr);
530: memcpy(ivec + 12, &rctr, sizeof rctr);
531:
532: if (io_ctr_AES(buf, rlen, &str, (u_char*) Key, ivec) == -1)
533: return NULL;
534:
535: return str;
536: }
537:
538: int
539: stopProcess(sched_root_task_t * __restrict root, proc_head_t * __restrict h, pid_t pid, sched_task_func_t func)
540: {
541: struct tagProc *p;
542:
543: FTRACE(3);
544:
545: SLIST_FOREACH(p, h, proc_next)
546: if (p->proc_pid == pid) {
547: break;
548: }
549: VERB(3) LOG("pid=%d found=%p\n", pid, p);
550: if (!p)
551: return 1;
552:
553: ioFreePTY(p->proc_pty, p->proc_ttyname);
554: if (p->proc_pty)
555: schedCancelby(root, NULL, CRITERIA_FD, (void*) ((intptr_t) p->proc_pty), NULL);
556:
557: p->proc_pty = 0;
558: p->proc_pid = 0;
559: p->proc_seq = 0;
560: p->proc_flg = ANSH_FLG_EOF;
561: p->proc_rlen_[FD2NET] = 0;
562:
563: schedCallOnce(root, func, p, p->proc_sock);
564: return 0;
565: }
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