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