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 2011/10/04 22:37:46 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_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:
140: /* check crc of packet */
141: crc = hdr->ansh_crc;
142: hdr->ansh_crc ^= hdr->ansh_crc;
143: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
144: if (crc != hdr->ansh_crc) {
145: VERB(3) LOG("Packet with wrong crc ...");
146: return ANSH_FLG_ERR;
147: }
148:
149: /* copy data */
150: if (data && datlen) {
151: memset(data, 0, *datlen);
152: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
153: memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
154: }
155:
156: if (id)
157: *id = ntohs(icmp->icmp_id);
158: return hdr->ansh_flg;
159: }
160:
161: int
162: icmpSend(int s, u_short id, char flg, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen)
163: {
164: u_char *pos, buf[USHRT_MAX] = { 0 };
165: struct icmp *icmp;
166: struct ansh_hdr *hdr;
167: int ret = 0;
168:
169: assert(data);
170: if ((sizeof buf - sizeof(struct icmp) + sizeof(struct ansh_hdr)) < datlen)
171: return ANSH_FLG_ERR;
172:
173: icmp = (struct icmp*) buf;
174: hdr = (struct ansh_hdr*) (buf + sizeof(struct icmp));
175: pos = buf + sizeof(struct icmp) + sizeof(struct ansh_hdr);
176:
177: memcpy(pos, data, datlen);
178:
179: hdr->ansh_ver = ANSH_VERSION;
180: hdr->ansh_flg = flg;
181: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
182: hdr->ansh_crc = 0;
183: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
184:
185: icmp->icmp_type = ICMP_ECHOREPLY;
186: icmp->icmp_code = ANSH_CODE;
187: icmp->icmp_id = htons(id);
188: icmp->icmp_seq = htons(datlen);
189: icmp->icmp_cksum = 0;
190: icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
191:
192: if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen,
193: 0, sa, salen)) == -1) {
194: ERR("Send sendto() #%d - %s", errno, strerror(errno));
195: return ANSH_FLG_ERR;
196: } else
197: VERB(4) LOG("Put packet with len=%d", ret);
198: if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
199: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
200: sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
201: return ANSH_FLG_ERR;
202: }
203:
204: return ret;
205: }
206:
207: int
208: pktSend(int s, u_short id, char flg, u_char *data, int datlen, struct ether_addr *ea)
209: {
210: u_char *pos, buf[USHRT_MAX] = { 0 };
211: struct ether_header *e = (struct ether_header*) buf;
212: struct ansh_hdr *hdr;
213: int ret = 0;
214:
215: assert(data);
216: if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
217: return ANSH_FLG_ERR;
218:
219: e->ether_type = htons(id);
220: memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);
221: hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
222: pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
223:
224: memcpy(pos, data, datlen);
225:
226: hdr->ansh_ver = ANSH_VERSION;
227: hdr->ansh_flg = flg;
228: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
229: hdr->ansh_crc = 0;
230: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
231:
232: if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
233: ERR("Send packet() #%d - %s", errno, strerror(errno));
234: return ANSH_FLG_ERR;
235: } else
236: VERB(4) LOG("Put packet with len=%d", ret);
237: if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
238: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
239: ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen);
240: return ANSH_FLG_ERR;
241: }
242:
243: return ret;
244: }
245:
246: char
247: pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)
248: {
249: int ret = 0;
250: struct bpf_hdr *bpf;
251: struct ether_header *e;
252: struct ansh_hdr *hdr;
253: u_char *buf;
254: u_int crc;
255:
256: if (!eth || !datlen)
257: return ANSH_FLG_ERR;
258:
259: if (!(buf = malloc(*datlen))) {
260: ERR("malloc() #%d - %s", errno, strerror(errno));
261: return ANSH_FLG_ERR;
262: }
263:
264: ret = read(s, buf, *datlen);
265: if (ret == -1) {
266: ERR("Receive packet() #%d - %s", errno, strerror(errno));
267: free(buf);
268: return ANSH_FLG_ERR;
269: } else
270: VERB(4) LOG("Get packet with len=%d", ret);
271:
272: /* check header len */
273: if (ret < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
274: VERB(1) LOG("Discard packet too short %d ...", ret);
275: free(buf);
276: return ANSH_FLG_ERR;
277: } else {
278: bpf = (struct bpf_hdr*) buf;
279: e = (struct ether_header*) (buf + bpf->bh_hdrlen);
280: memcpy(eth, e, ETHER_HDR_LEN);
281: hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
282: }
283:
284: /* check version and total size of packet */
285: if (hdr->ansh_ver != ANSH_VERSION) {
286: VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
287: free(buf);
288: return ANSH_FLG_ERR;
289: }
290: /* check crc of packet */
291: crc = hdr->ansh_crc;
292: hdr->ansh_crc ^= hdr->ansh_crc;
293: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
294: if (crc != hdr->ansh_crc) {
295: VERB(3) LOG("Packet with wrong crc ...");
296: free(buf);
297: return ANSH_FLG_ERR;
298: }
299:
300: /* copy data */
301: if (data) {
302: memset(data, 0, *datlen);
303: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
304: memcpy(data, hdr + sizeof(struct ansh_hdr), *datlen);
305: }
306:
307: ret = (char) hdr->ansh_flg;
308: free(buf);
309: return (char) ret;
310: }
311:
312: void *
313: TOfunc(sched_task_t *task)
314: {
315: struct tagProc *proc;
316:
317: FTRACE(3);
318:
319: /* not found argument, drop data */
320: if (!(proc = TASK_ARG(task)))
321: return (void*) -1;
322:
323: if (proc->proc_pid)
324: kill(proc->proc_pid, SIGTERM);
325:
326: return NULL;
327: }
328:
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