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