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