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