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.1 2011/10/07 13:41:26 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:
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
104: icmpRecv(int s, u_short * __restrict id, u_int * __restrict crypted, u_char * __restrict data,
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: }
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: }
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
174: icmpSend(int s, u_short id, char flg, u_int crypted, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen)
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));
194: hdr->ansh_nonce = htonl(crypted);
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
221: pktSend(int s, u_short id, char flg, u_char *data, int datlen, struct ether_addr *ea)
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));
242: hdr->ansh_crc = 0;
243: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
244:
245: if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
246: ERR("Send packet() #%d - %s", errno, strerror(errno));
247: return ANSH_FLG_ERR;
248: } else
249: VERB(4) LOG("Put packet with len=%d", ret);
250: if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
251: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
252: ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen);
253: return ANSH_FLG_ERR;
254: }
255:
256: return ret;
257: }
258:
259: char
260: pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)
261: {
262: int ret = 0;
263: struct bpf_hdr *bpf;
264: struct ether_header *e;
265: struct ansh_hdr *hdr;
266: u_char *buf;
267: u_int crc;
268:
269: if (!eth || !datlen)
270: return ANSH_FLG_ERR;
271:
272: if (!(buf = malloc(*datlen))) {
273: ERR("malloc() #%d - %s", errno, strerror(errno));
274: return ANSH_FLG_ERR;
275: }
276:
277: ret = read(s, buf, *datlen);
278: if (ret == -1) {
279: ERR("Receive packet() #%d - %s", errno, strerror(errno));
280: free(buf);
281: return ANSH_FLG_ERR;
282: } else
283: VERB(4) LOG("Get packet with len=%d", ret);
284:
285: /* check header len */
286: if (ret < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
287: VERB(1) LOG("Discard packet too short %d ...", ret);
288: free(buf);
289: return ANSH_FLG_ERR;
290: } else {
291: bpf = (struct bpf_hdr*) buf;
292: e = (struct ether_header*) (buf + bpf->bh_hdrlen);
293: memcpy(eth, e, ETHER_HDR_LEN);
294: hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
295: }
296:
297: /* check version and total size of packet */
298: if (hdr->ansh_ver != ANSH_VERSION) {
299: VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
300: free(buf);
301: return ANSH_FLG_ERR;
302: }
303: /* check crc of packet */
304: crc = hdr->ansh_crc;
305: hdr->ansh_crc ^= hdr->ansh_crc;
306: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
307: if (crc != hdr->ansh_crc) {
308: VERB(3) LOG("Packet with wrong crc ...");
309: free(buf);
310: return ANSH_FLG_ERR;
311: }
312:
313: /* copy data */
314: if (data) {
315: memset(data, 0, *datlen);
316: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
317: memcpy(data, hdr + sizeof(struct ansh_hdr), *datlen);
318: }
319:
320: ret = (char) hdr->ansh_flg;
321: free(buf);
322: return (char) ret;
323: }
324:
325: void *
326: TOfunc(sched_task_t *task)
327: {
328: struct tagProc *proc;
329:
330: FTRACE(3);
331:
332: /* not found argument, drop data */
333: if (!(proc = TASK_ARG(task)))
334: return (void*) -1;
335:
336: if (proc->proc_pid)
337: kill(proc->proc_pid, SIGTERM);
338:
339: return NULL;
340: }
341:
342: u_char *
343: cryptBuffer(u_char *buf, int rlen, u_int ctr)
344: {
345: u_char *str, ivec[AES_BLOCK_SIZE] = { 0 };
346: u_int rctr = htonl(ctr);
347:
348: FTRACE(3);
349:
350: if (!buf)
351: return NULL;
352:
353: memcpy(ivec, &ctr, sizeof ctr);
354: memcpy(ivec + 4, &rctr, sizeof rctr);
355: memcpy(ivec + 8, &ctr, sizeof ctr);
356: memcpy(ivec + 12, &rctr, sizeof rctr);
357:
358: if (io_ctr_AES(buf, rlen, &str, (u_char*) "_ansh_ELWIX_", ivec) == -1)
359: return NULL;
360:
361: return str;
362: }
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