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.1.1.1.2.7 2011/10/13 15:24:17 misho Exp $
7: *
8: *************************************************************************/
9: #include "global.h"
10:
11:
12: void
13: Get1stEth(char *psDev, int devlen)
14: {
15: struct ifaddrs *ifa;
16:
17: assert(psDev);
18: assert(devlen > 0);
19:
20: getifaddrs(&ifa);
21: strlcpy(psDev, ifa->ifa_name, devlen);
22: freeifaddrs(ifa);
23: }
24:
25: int
26: PrepareL2(const char *psDev, int *bpflen)
27: {
28: int h, n = 1;
29: register int i;
30: char szStr[STRSIZ];
31: struct ifreq ifr;
32: struct bpf_program fcode = { 0 };
33: struct bpf_insn insns[] = {
34: BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
35: BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ANSH_ID, 0, 1),
36: BPF_STMT(BPF_RET + BPF_K, -1),
37: BPF_STMT(BPF_RET + BPF_K, 0),
38: };
39:
40: FTRACE(3);
41: assert(psDev);
42:
43: fcode.bf_len = sizeof(insns) / sizeof(struct bpf_insn);
44: fcode.bf_insns = insns;
45:
46: for (i = 0; i < 10; i++) {
47: memset(szStr, 0, sizeof szStr);
48: snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
49: h = open(szStr, O_RDWR);
50: if (h > 2)
51: break;
52: }
53: if (h < 3) {
54: printf("Error:: open bpf %s #%d - %s\n", szStr, errno, strerror(errno));
55: return -1;
56: }
57:
58: if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {
59: printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
60: close(h);
61: return -1;
62: }
63: n = USHRT_MAX + 1;
64: if (ioctl(h, BIOCSBLEN, &n) == -1) {
65: printf("Error:: set buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
66: close(h);
67: return -1;
68: }
69: strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);
70: if (ioctl(h, BIOCSETIF, &ifr) == -1) {
71: printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
72: close(h);
73: return -1;
74: }
75: if (ioctl(h, BIOCSETF, &fcode) == -1) {
76: printf("Error:: set filter interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
77: close(h);
78: return -1;
79: }
80: if (ioctl(h, BIOCGBLEN, bpflen) == -1) {
81: printf("Error:: get buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
82: close(h);
83: return -1;
84: }
85:
86: /*
87: n = fcntl(h, F_GETFL);
88: fcntl(h, F_SETFL, n | O_NONBLOCK);
89: */
90:
91: VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
92: return h;
93: }
94:
95: int
96: PrepareL3(const struct sockaddr *sa, int *bpflen)
97: {
98: int h, n = 1;
99:
100: FTRACE(3);
101: assert(sa);
102:
103: h = socket(sa->sa_family, SOCK_RAW, IPPROTO_ICMP);
104: if (h == -1) {
105: printf("Error:: Cant open raw socket #%d - %s\n", errno, strerror(errno));
106: return -1;
107: }
108: /*
109: if (setsockopt(h, SOL_SOCKET, SO_REUSEADDR, &n, sizeof n) == -1) {
110: printf("Error:: Cant set raw socket #%d - %s\n", errno, strerror(errno));
111: close(h);
112: return -1;
113: }
114: */
115: if (bind(h, sa, sizeof(struct sockaddr)) == -1) {
116: printf("Error:: Cant bind to raw socket #%d - %s\n", errno, strerror(errno));
117: close(h);
118: return -1;
119: }
120:
121: n = fcntl(h, F_GETFL);
122: fcntl(h, F_SETFL, n | O_NONBLOCK);
123:
124: *bpflen = USHRT_MAX;
125: VERB(3) LOG("Openned socket handle %d", h);
126: return h;
127: }
128:
129: char
130: icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted,
131: u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
132: {
133: int ret = 0;
134: struct icmp *icmp;
135: struct ansh_hdr *hdr;
136: u_char buf[USHRT_MAX] = { 0 };
137: u_int crc;
138:
139: ret = recvfrom(s, buf, sizeof buf, 0, sa, salen);
140: if (ret == -1) {
141: ERR("Receive recvfrom() #%d - %s", errno, strerror(errno));
142: return ANSH_FLG_ERR;
143: } else
144: VERB(4) LOG("Get packet with len=%d", ret);
145:
146: /* check header len */
147: if (ret < (sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr))) {
148: VERB(1) LOG("Discard packet too short %d ...", ret);
149: return ANSH_FLG_ERR;
150: } else
151: icmp = (struct icmp*) (buf + sizeof(struct ip));
152:
153: /* check echo magic ansh code */
154: if (icmp->icmp_type != ICMP_ECHOREPLY || icmp->icmp_code != ANSH_CODE) {
155: VERB(3) LOG("Packet isnt for me %d ... icmp_code=%d", ret, icmp->icmp_code);
156: return ANSH_FLG_ERR;
157: } else
158: hdr = (struct ansh_hdr*) (buf + sizeof(struct ip) + sizeof(struct icmp));
159:
160: /* check version and total size of packet */
161: if (hdr->ansh_ver != ANSH_VERSION) {
162: VERB(3) LOG("Packet with wrong version ...");
163: return ANSH_FLG_ERR;
164: }
165: if (crypted) {
166: if (hdr->ansh_nonce && !*crypted) {
167: VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
168: return ANSH_FLG_ERR;
169: }
170: if (!hdr->ansh_nonce && *crypted) {
171: VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
172: return ANSH_FLG_ERR;
173: }
174:
175: *crypted = ntohl(hdr->ansh_nonce);
176: }
177:
178: /* check crc of packet */
179: crc = hdr->ansh_crc;
180: hdr->ansh_crc ^= hdr->ansh_crc;
181: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
182: if (crc != hdr->ansh_crc) {
183: VERB(3) LOG("Packet with wrong crc ...");
184: return ANSH_FLG_ERR;
185: }
186:
187: /* copy data */
188: if (data && datlen) {
189: memset(data, 0, *datlen);
190: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
191: memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
192: }
193:
194: if (seq)
195: *seq = ntohl(hdr->ansh_seq);
196: if (id)
197: *id = ntohs(icmp->icmp_id);
198: return hdr->ansh_flg;
199: }
200:
201: int
202: icmpSend(int s, u_int seq, u_short id, char flg, u_int crypted, u_char *data, int datlen,
203: struct sockaddr *sa, socklen_t salen)
204: {
205: u_char *pos, buf[USHRT_MAX] = { 0 };
206: struct icmp *icmp;
207: struct ansh_hdr *hdr;
208: int ret = 0;
209:
210: assert(data);
211: if ((sizeof buf - sizeof(struct icmp) + sizeof(struct ansh_hdr)) < datlen)
212: return ANSH_FLG_ERR;
213:
214: icmp = (struct icmp*) buf;
215: hdr = (struct ansh_hdr*) (buf + sizeof(struct icmp));
216: pos = buf + sizeof(struct icmp) + sizeof(struct ansh_hdr);
217:
218: memcpy(pos, data, datlen);
219:
220: hdr->ansh_ver = ANSH_VERSION;
221: hdr->ansh_flg = flg;
222: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
223: hdr->ansh_nonce = htonl(crypted);
224: hdr->ansh_seq = htonl(seq);
225: hdr->ansh_crc = 0;
226: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
227:
228: icmp->icmp_type = ICMP_ECHOREPLY;
229: icmp->icmp_code = ANSH_CODE;
230: icmp->icmp_id = htons(id);
231: icmp->icmp_seq = htons(datlen);
232: icmp->icmp_cksum = 0;
233: icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
234:
235: if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen,
236: 0, sa, salen)) == -1) {
237: ERR("Send sendto() #%d - %s", errno, strerror(errno));
238: return ANSH_FLG_ERR;
239: } else
240: VERB(4) LOG("Put packet with len=%d", ret);
241: if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
242: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
243: sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
244: return ANSH_FLG_ERR;
245: }
246:
247: return ret;
248: }
249:
250: static int
251: _pkt_Send(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
252: {
253: u_char *pos, buf[USHRT_MAX] = { 0 };
254: struct ether_header *e = (struct ether_header*) buf;
255: struct ansh_hdr *hdr;
256: int ret = 0;
257:
258: assert(data);
259: if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
260: return ANSH_FLG_ERR;
261:
262: e->ether_type = ntohs(ANSH_ID);
263: memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);
264: hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
265: pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
266:
267: memcpy(pos, data, datlen);
268:
269: hdr->ansh_ver = ANSH_VERSION;
270: hdr->ansh_flg = flg;
271: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
272: hdr->ansh_nonce = htonl(crypted);
273: hdr->ansh_seq = htonl(seq);
274: hdr->ansh_crc = 0;
275: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
276:
277: if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
278: ERR("Send packet() #%d - %s", errno, strerror(errno));
279: return ANSH_FLG_ERR;
280: } else
281: VERB(4) LOG("Put packet with len=%d", ret);
282: if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
283: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
284: ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen);
285: return ANSH_FLG_ERR;
286: }
287:
288: return ret;
289: }
290:
291: int
292: pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
293: {
294: int wlen, ret = 0;
295: u_char *pos = data;
296:
297: while (datlen > -1) {
298: wlen = _pkt_Send(s, seq, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
299: if (wlen == -1)
300: return -1;
301: else {
302: pos += wlen;
303: datlen -= wlen;
304: ret += wlen;
305: }
306: }
307:
308: return ret;
309: }
310:
311: static char
312: _pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict seq, u_int * __restrict crypted,
313: u_char * __restrict data, int * __restrict datlen,
314: u_char ** __restrict next, int * __restrict nextlen)
315: {
316: int bias;
317: struct bpf_hdr *bpf;
318: struct ansh_hdr *hdr;
319: u_int crc;
320:
321: if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
322: VERB(1) LOG("Discard packet too short %d ...", rlen);
323: return ANSH_FLG_ERR;
324: } else {
325: bpf = (struct bpf_hdr*) buf;
326: hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
327: }
328:
329: /* slice readed data to packets */
330: if ((bias = BPF_WORDALIGN(bpf->bh_hdrlen + bpf->bh_caplen)) < rlen) {
331: *next = buf + bias;
332: *nextlen = rlen - bias;
333: } else {
334: *next = NULL;
335: *nextlen = 0;
336: }
337:
338: /* check version and total size of packet */
339: if (hdr->ansh_ver != ANSH_VERSION) {
340: VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
341: return ANSH_FLG_ERR;
342: }
343: if (crypted) {
344: if (hdr->ansh_nonce && !*crypted) {
345: VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
346: return ANSH_FLG_ERR;
347: }
348: if (!hdr->ansh_nonce && *crypted) {
349: VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
350: return ANSH_FLG_ERR;
351: }
352:
353: *crypted = ntohl(hdr->ansh_nonce);
354: }
355:
356: /* check crc of packet */
357: crc = hdr->ansh_crc;
358: hdr->ansh_crc ^= hdr->ansh_crc;
359: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
360: if (crc != hdr->ansh_crc) {
361: VERB(3) LOG("Packet with wrong crc ...");
362: return ANSH_FLG_ERR;
363: }
364:
365: /* select data */
366: if (data) {
367: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
368: memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen);
369: }
370:
371: if (seq)
372: *seq = ntohl(hdr->ansh_seq);
373: return hdr->ansh_flg;
374: }
375:
376: char
377: pktRecv(int s, u_int * __restrict seq, u_int * __restrict crypted, u_char * __restrict data,
378: int * __restrict datlen, struct ether_header *eth)
379: {
380: u_char *buf, *next, *ptr, *pos = data;
381: int nextlen, rlen, buflen, ptrlen;
382: char flg;
383: struct bpf_hdr *bpf;
384: struct ether_header *e;
385:
386: if (!eth || !data || !datlen)
387: return ANSH_FLG_ERR;
388: else
389: memset(data, 0, *datlen);
390:
391: if (!(buf = malloc(*datlen))) {
392: ERR("malloc() #%d - %s", errno, strerror(errno));
393: return ANSH_FLG_ERR;
394: }
395:
396: rlen = read(s, buf, *datlen);
397: if (rlen == -1) {
398: ERR("Receive packet() #%d - %s", errno, strerror(errno));
399: free(buf);
400: return ANSH_FLG_ERR;
401: } else
402: VERB(4) LOG("Get packet with len=%d", rlen);
403:
404: /* check header len */
405: if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
406: VERB(1) LOG("Discard packet too short %d ...", rlen);
407: free(buf);
408: return ANSH_FLG_ERR;
409: } else {
410: bpf = (struct bpf_hdr*) buf;
411: e = (struct ether_header*) (buf + bpf->bh_hdrlen);
412: memcpy(eth, e, ETHER_HDR_LEN);
413: }
414:
415: ptr = next = buf;
416: ptrlen = nextlen = rlen;
417: if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) {
418: free(buf);
419: return ANSH_FLG_ERR;
420: } else {
421: pos += buflen;
422: *datlen = buflen;
423: ptr = next;
424: ptrlen = nextlen;
425: }
426: /* get additional packets from buffer */
427: while (next && nextlen > 0)
428: if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1)
429: break;
430: else {
431: pos += buflen;
432: *datlen += buflen;
433: ptr = next;
434: ptrlen = nextlen;
435: }
436:
437: free(buf);
438:
439: return flg;
440: }
441:
442: void *
443: TOfunc(sched_task_t *task)
444: {
445: struct tagProc *proc;
446:
447: FTRACE(3);
448:
449: /* not found argument, drop data */
450: if (!(proc = TASK_ARG(task)))
451: return (void*) -1;
452:
453: if (proc->proc_pid)
454: kill(proc->proc_pid, SIGTERM);
455:
456: return NULL;
457: }
458:
459: u_char *
460: cryptBuffer(u_char *buf, int rlen, u_int ctr)
461: {
462: u_char *str, ivec[AES_BLOCK_SIZE] = { 0 };
463: u_int rctr = htonl(ctr);
464:
465: FTRACE(3);
466:
467: if (!buf)
468: return NULL;
469:
470: memcpy(ivec, &ctr, sizeof ctr);
471: memcpy(ivec + 4, &rctr, sizeof rctr);
472: memcpy(ivec + 8, &ctr, sizeof ctr);
473: memcpy(ivec + 12, &rctr, sizeof rctr);
474:
475: if (io_ctr_AES(buf, rlen, &str, (u_char*) "_ansh_ELWIX_", ivec) == -1)
476: return NULL;
477:
478: return str;
479: }
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