1: /*
2: * $Id: libnet_pblock.c,v 1.1.1.2 2013/07/22 11:54:42 misho Exp $
3: *
4: * libnet
5: * libnet_pblock.c - Memory protocol block routines.
6: *
7: * Copyright (c) 1998 - 2004 Mike D. Schiffman <mike@infonexus.com>
8: * All rights reserved.
9: *
10: * Redistribution and use in source and binary forms, with or without
11: * modification, are permitted provided that the following conditions
12: * are met:
13: * 1. Redistributions of source code must retain the above copyright
14: * notice, this list of conditions and the following disclaimer.
15: * 2. Redistributions in binary form must reproduce the above copyright
16: * notice, this list of conditions and the following disclaimer in the
17: * documentation and/or other materials provided with the distribution.
18: *
19: * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
20: * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21: * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22: * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
23: * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24: * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
25: * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
26: * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27: * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
28: * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
29: * SUCH DAMAGE.
30: *
31: */
32:
33: #if (HAVE_CONFIG_H)
34: #include "../include/config.h"
35: #endif
36: #if (!(_WIN32) || (__CYGWIN__))
37: #include "../include/libnet.h"
38: #else
39: #include "../include/win32/libnet.h"
40: #endif
41: #include <assert.h>
42:
43: libnet_pblock_t *
44: libnet_pblock_probe(libnet_t *l, libnet_ptag_t ptag, uint32_t b_len, uint8_t type)
45: {
46: int offset;
47: libnet_pblock_t *p;
48:
49: if (ptag == LIBNET_PTAG_INITIALIZER)
50: {
51: return libnet_pblock_new(l, b_len);
52: }
53:
54: /*
55: * Update this pblock, don't create a new one. Note that if the
56: * new packet size is larger than the old one we will do a malloc.
57: */
58: p = libnet_pblock_find(l, ptag);
59:
60: if (p == NULL)
61: {
62: /* err msg set in libnet_pblock_find() */
63: return (NULL);
64: }
65: if (p->type != type)
66: {
67: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
68: "%s(): ptag refers to different type than expected (0x%x != 0x%x)",
69: __func__, p->type, type);
70: return (NULL);
71: }
72: /*
73: * If size is greater than the original block of memory, we need
74: * to malloc more memory. Should we use realloc?
75: */
76: if (b_len > p->b_len)
77: {
78: offset = b_len - p->b_len; /* how many bytes larger new pblock is */
79: free(p->buf);
80: p->buf = malloc(b_len);
81: if (p->buf == NULL)
82: {
83: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
84: "%s(): can't resize pblock buffer: %s\n", __func__,
85: strerror(errno));
86: return (NULL);
87: }
88: memset(p->buf, 0, b_len);
89: p->h_len += offset; /* new length for checksums */
90: p->b_len = b_len; /* new buf len */
91: l->total_size += offset;
92: }
93: else
94: {
95: offset = p->b_len - b_len;
96: p->h_len -= offset; /* new length for checksums */
97: p->b_len = b_len; /* new buf len */
98: l->total_size -= offset;
99: }
100: p->copied = 0; /* reset copied counter */
101:
102: return (p);
103: }
104:
105: static void* zmalloc(libnet_t* l, uint32_t size, const char* func)
106: {
107: void* v = malloc(size);
108: if(v)
109: memset(v, 0, size);
110: else
111: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE, "%s(): malloc(): %s\n", func,
112: strerror(errno));
113: return v;
114: }
115:
116: libnet_pblock_t *
117: libnet_pblock_new(libnet_t *l, uint32_t b_len)
118: {
119: libnet_pblock_t *p = zmalloc(l, sizeof(libnet_pblock_t), __func__);
120: if(!p)
121: return NULL;
122:
123: p->buf = zmalloc(l, b_len, __func__);
124:
125: if(!p->buf)
126: {
127: free(p);
128: return NULL;
129: }
130:
131: p->b_len = b_len;
132:
133: l->total_size += b_len;
134: l->n_pblocks++;
135:
136: /* make the head node if it doesn't exist */
137: if (l->protocol_blocks == NULL)
138: {
139: l->protocol_blocks = p;
140: l->pblock_end = p;
141: }
142: else
143: {
144: l->pblock_end->next = p;
145: p->prev = l->pblock_end;
146: l->pblock_end = p;
147: }
148:
149: return p;
150: }
151:
152: int
153: libnet_pblock_swap(libnet_t *l, libnet_ptag_t ptag1, libnet_ptag_t ptag2)
154: {
155: libnet_pblock_t *p1, *p2;
156:
157: p1 = libnet_pblock_find(l, ptag1);
158: p2 = libnet_pblock_find(l, ptag2);
159: if (p1 == NULL || p2 == NULL)
160: {
161: /* error set elsewhere */
162: return (-1);
163: }
164:
165: p2->prev = p1->prev;
166: p1->next = p2->next;
167: p2->next = p1;
168: p1->prev = p2;
169:
170: if (p1->next)
171: {
172: p1->next->prev = p1;
173: }
174:
175: if (p2->prev)
176: {
177: p2->prev->next = p2;
178: }
179: else
180: {
181: /* first node on the list */
182: l->protocol_blocks = p2;
183: }
184:
185: if (l->pblock_end == p2)
186: {
187: l->pblock_end = p1;
188: }
189: return (1);
190: }
191:
192: static void libnet_pblock_remove_from_list(libnet_t *l, libnet_pblock_t *p)
193: {
194: if (p->prev)
195: {
196: p->prev->next = p->next;
197: }
198: else
199: {
200: l->protocol_blocks = p->next;
201: }
202:
203: if (p->next)
204: {
205: p->next->prev = p->prev;
206: }
207: else
208: {
209: l->pblock_end = p->prev;
210: }
211: }
212:
213: int
214: libnet_pblock_insert_before(libnet_t *l, libnet_ptag_t ptag1,
215: libnet_ptag_t ptag2)
216: {
217: libnet_pblock_t *p1, *p2;
218:
219: p1 = libnet_pblock_find(l, ptag1);
220: p2 = libnet_pblock_find(l, ptag2);
221: if (p1 == NULL || p2 == NULL)
222: {
223: /* error set elsewhere */
224: return (-1);
225: }
226:
227: /* check for already present before */
228: if(p2->next == p1)
229: return 1;
230:
231: libnet_pblock_remove_from_list(l, p2);
232:
233: /* insert p2 into list */
234: p2->prev = p1->prev;
235: p2->next = p1;
236: p1->prev = p2;
237:
238: if (p2->prev)
239: {
240: p2->prev->next = p2;
241: }
242: else
243: {
244: /* first node on the list */
245: l->protocol_blocks = p2;
246: }
247:
248: return (1);
249: }
250:
251: libnet_pblock_t *
252: libnet_pblock_find(libnet_t *l, libnet_ptag_t ptag)
253: {
254: libnet_pblock_t *p;
255:
256: for (p = l->protocol_blocks; p; p = p->next)
257: {
258: if (p->ptag == ptag)
259: {
260: return (p);
261: }
262: }
263: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
264: "%s(): couldn't find protocol block\n", __func__);
265: return (NULL);
266: }
267:
268: int
269: libnet_pblock_append(libnet_t *l, libnet_pblock_t *p, const void *buf, uint32_t len)
270: {
271: if (len && !buf)
272: {
273: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
274: "%s(): payload inconsistency\n", __func__);
275: return -1;
276: }
277:
278: if (p->copied + len > p->b_len)
279: {
280: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
281: "%s(): memcpy would overflow buffer\n", __func__);
282: return (-1);
283: }
284: memcpy(p->buf + p->copied, buf, len);
285: p->copied += len;
286: return (1);
287: }
288:
289: void
290: libnet_pblock_setflags(libnet_pblock_t *p, uint8_t flags)
291: {
292: p->flags = flags;
293: }
294:
295: /* FIXME both ptag setting and end setting should be done in pblock new and/or pblock probe. */
296: libnet_ptag_t
297: libnet_pblock_update(libnet_t *l, libnet_pblock_t *p, uint32_t h_len, uint8_t type)
298: {
299: p->type = type;
300: p->ptag = ++(l->ptag_state);
301: p->h_len = h_len;
302: l->pblock_end = p; /* point end of pblock list here */
303:
304: return (p->ptag);
305: }
306:
307: static int pblock_is_ip(libnet_pblock_t* p)
308: {
309: return p->type == LIBNET_PBLOCK_IPV4_H || p->type == LIBNET_PBLOCK_IPV6_H;
310: }
311:
312: /* q is either an ip hdr, or is followed by an ip hdr. return the offset
313: * from end of packet. if there is no offset, we'll return the total size,
314: * and things will break later
315: */
316: static int calculate_ip_offset(libnet_t* l, libnet_pblock_t* q)
317: {
318: int ip_offset = 0;
319: libnet_pblock_t* p = l->protocol_blocks;
320: for(; p && p != q; p = p->next) {
321: ip_offset += p->b_len;
322: }
323: assert(p == q); /* if not true, then q is not a pblock! */
324:
325: for(; p; p = p->next) {
326: ip_offset += p->b_len;
327: if(pblock_is_ip(p))
328: break;
329: }
330:
331: return ip_offset;
332: }
333:
334: int
335: libnet_pblock_coalesce(libnet_t *l, uint8_t **packet, uint32_t *size)
336: {
337: /*
338: * Determine the offset required to keep memory aligned (strict
339: * architectures like solaris enforce this, but's a good practice
340: * either way). This is only required on the link layer with the
341: * 14 byte ethernet offset (others are similarly unkind).
342: */
343: if (l->injection_type == LIBNET_LINK ||
344: l->injection_type == LIBNET_LINK_ADV)
345: {
346: /* 8 byte alignment should work */
347: l->aligner = 8 - (l->link_offset % 8);
348: }
349: else
350: {
351: l->aligner = 0;
352: }
353:
354: if(!l->total_size && !l->aligner) {
355: /* Avoid allocating zero bytes of memory, it perturbs electric fence. */
356: *packet = malloc(1);
357: **packet =1;
358: } else {
359: *packet = malloc(l->aligner + l->total_size);
360: }
361: if (*packet == NULL)
362: {
363: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE, "%s(): malloc(): %s\n",
364: __func__, strerror(errno));
365: return (-1);
366: }
367:
368: memset(*packet, 0, l->aligner + l->total_size);
369:
370: if (l->injection_type == LIBNET_RAW4 &&
371: l->pblock_end->type == LIBNET_PBLOCK_IPV4_H)
372: {
373: libnet_pblock_setflags(l->pblock_end, LIBNET_PBLOCK_DO_CHECKSUM);
374: }
375:
376: /* additional sanity checks to perform if we're not in advanced mode */
377: if (!(l->injection_type & LIBNET_ADV_MASK))
378: {
379: switch (l->injection_type)
380: {
381: case LIBNET_LINK:
382: if ((l->pblock_end->type != LIBNET_PBLOCK_TOKEN_RING_H) &&
383: (l->pblock_end->type != LIBNET_PBLOCK_FDDI_H) &&
384: (l->pblock_end->type != LIBNET_PBLOCK_ETH_H) &&
385: (l->pblock_end->type != LIBNET_PBLOCK_802_1Q_H) &&
386: (l->pblock_end->type != LIBNET_PBLOCK_ISL_H) &&
387: (l->pblock_end->type != LIBNET_PBLOCK_802_3_H))
388: {
389: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
390: "%s(): packet assembly cannot find a layer 2 header\n",
391: __func__);
392: goto err;
393: }
394: break;
395: case LIBNET_RAW4:
396: if ((l->pblock_end->type != LIBNET_PBLOCK_IPV4_H))
397: {
398: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
399: "%s(): packet assembly cannot find an IPv4 header\n",
400: __func__);
401: goto err;
402: }
403: break;
404: case LIBNET_RAW6:
405: if ((l->pblock_end->type != LIBNET_PBLOCK_IPV6_H))
406: {
407: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
408: "%s(): packet assembly cannot find an IPv6 header\n",
409: __func__);
410: goto err;
411: }
412: break;
413: default:
414: /* we should not end up here ever */
415: snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
416: "%s(): suddenly the dungeon collapses -- you die\n",
417: __func__);
418: goto err;
419: break;
420: }
421: }
422:
423: /* Build packet from end to start. */
424: {
425: /*
426: From top to bottom, go through pblocks pairwise:
427:
428: p is the currently being copied pblock, and steps through every block
429: q is the prev pblock to p that needs checksumming, it will
430: not step through every block as p does, it will skip any that do not
431: need checksumming.
432: n offset from start of packet to beginning of block we are writing
433:
434: q is NULL on first iteration
435: p is NULL on last iteration
436:
437: Checksums are done on q, to give p a chance to be copied over, since
438: checksumming q can require a lower-level header to be encoded, in the
439: case of IP protocols (which are the only kinds handled by libnet's
440: checksum implementation).
441:
442: This is very obscure, or would be much more clear if it was done in
443: two loops.
444: */
445: libnet_pblock_t *q = NULL;
446: libnet_pblock_t *p = NULL;
447: uint32_t n;
448:
449: for (n = l->aligner + l->total_size, p = l->protocol_blocks; p || q; )
450: {
451: if (q)
452: {
453: p = p->next;
454: }
455: if (p)
456: {
457: n -= p->b_len;
458: /* copy over the packet chunk */
459: memcpy(*packet + n, p->buf, p->b_len);
460: }
461: #if 0
462: printf("-- n %d/%d cksum? %d\n", n, l->aligner + l->total_size,
463: q &&
464: (p == NULL || (p->flags & LIBNET_PBLOCK_DO_CHECKSUM)) &&
465: (q->flags & LIBNET_PBLOCK_DO_CHECKSUM));
466: if(q)
467: {
468: printf(" iph %d/%d offset -%d\n",
469: (l->total_size + l->aligner) - q->ip_offset,
470: l->total_size + l->aligner,
471: q->ip_offset
472: );
473: }
474: if (p)
475: {
476: printf("p %p ptag %d b_len %d h_len %d cksum? %d type %s\n",
477: p, p->ptag,
478: p->b_len, p->h_len,
479: p->flags & LIBNET_PBLOCK_DO_CHECKSUM,
480: libnet_diag_dump_pblock_type(p->type)
481: );
482: }
483: if (q)
484: {
485: printf("q %p ptag %d b_len %d h_len %d cksum? %d type %s\n",
486: q, q->ptag,
487: q->b_len, q->h_len,
488: q->flags & LIBNET_PBLOCK_DO_CHECKSUM,
489: libnet_diag_dump_pblock_type(q->type)
490: );
491: }
492: #endif
493: if (q)
494: {
495: if (p == NULL || (p->flags & LIBNET_PBLOCK_DO_CHECKSUM))
496: {
497: if (q->flags & LIBNET_PBLOCK_DO_CHECKSUM)
498: {
499: uint32_t c;
500: uint8_t* end = *packet + l->aligner + l->total_size;
501: uint8_t* beg = *packet + n;
502: int ip_offset = calculate_ip_offset(l, q);
503: uint8_t* iph = end - ip_offset;
504: #if 0
505: printf("p %d/%s q %d/%s offset calculated %d\n",
506: p ? p->ptag : -1, p ? libnet_diag_dump_pblock_type(p->type) : "nil",
507: q->ptag, libnet_diag_dump_pblock_type(q->type),
508: ip_offset);
509: #endif
510: c = libnet_inet_checksum(l, iph,
511: libnet_pblock_p2p(q->type), q->h_len,
512: beg, end);
513: if (c == -1)
514: {
515: /* err msg set in libnet_do_checksum() */
516: goto err;
517: }
518: }
519: q = p;
520: }
521: }
522: else
523: {
524: q = p;
525: }
526: }
527: }
528: *size = l->aligner + l->total_size;
529:
530: /*
531: * Set the packet pointer to the true beginning of the packet and set
532: * the size for transmission.
533: */
534: if ((l->injection_type == LIBNET_LINK ||
535: l->injection_type == LIBNET_LINK_ADV) && l->aligner)
536: {
537: *packet += l->aligner;
538: *size -= l->aligner;
539: }
540: return (1);
541:
542: err:
543: free(*packet);
544: *packet = NULL;
545: return (-1);
546: }
547:
548: void
549: libnet_pblock_delete(libnet_t *l, libnet_pblock_t *p)
550: {
551: if (p)
552: {
553: l->total_size -= p->b_len;
554: l->n_pblocks--;
555:
556: libnet_pblock_remove_from_list(l, p);
557:
558: if (p->buf)
559: {
560: free(p->buf);
561: p->buf = NULL;
562: }
563:
564: free(p);
565: }
566: }
567:
568: int
569: libnet_pblock_p2p(uint8_t type)
570: {
571: /* for checksum; return the protocol number given a pblock type*/
572: switch (type)
573: {
574: case LIBNET_PBLOCK_CDP_H:
575: return (LIBNET_PROTO_CDP);
576: case LIBNET_PBLOCK_ICMPV4_H:
577: case LIBNET_PBLOCK_ICMPV4_ECHO_H:
578: case LIBNET_PBLOCK_ICMPV4_MASK_H:
579: case LIBNET_PBLOCK_ICMPV4_UNREACH_H:
580: case LIBNET_PBLOCK_ICMPV4_TIMXCEED_H:
581: case LIBNET_PBLOCK_ICMPV4_REDIRECT_H:
582: case LIBNET_PBLOCK_ICMPV4_TS_H:
583: return (IPPROTO_ICMP);
584: case LIBNET_PBLOCK_ICMPV6_H:
585: case LIBNET_PBLOCK_ICMPV6_ECHO_H:
586: case LIBNET_PBLOCK_ICMPV6_UNREACH_H:
587: case LIBNET_PBLOCK_ICMPV6_NDP_NSOL_H:
588: case LIBNET_PBLOCK_ICMPV6_NDP_NADV_H:
589: return (IPPROTO_ICMPV6);
590: case LIBNET_PBLOCK_IGMP_H:
591: return (IPPROTO_IGMP);
592: case LIBNET_PBLOCK_IPV4_H:
593: return (IPPROTO_IP);
594: case LIBNET_PBLOCK_IPV6_H:
595: return (IPPROTO_IPV6);
596: case LIBNET_ISL_H:
597: return (LIBNET_PROTO_ISL);
598: case LIBNET_PBLOCK_OSPF_H:
599: return (IPPROTO_OSPF);
600: case LIBNET_PBLOCK_LS_RTR_H:
601: return (IPPROTO_OSPF_LSA);
602: case LIBNET_PBLOCK_TCP_H:
603: return (IPPROTO_TCP);
604: case LIBNET_PBLOCK_UDP_H:
605: return (IPPROTO_UDP);
606: case LIBNET_PBLOCK_VRRP_H:
607: return (IPPROTO_VRRP);
608: case LIBNET_PBLOCK_GRE_H:
609: return (IPPROTO_GRE);
610: default:
611: return (-1);
612: }
613: }
614:
615: void
616: libnet_pblock_record_ip_offset(libnet_t *l, libnet_pblock_t *p)
617: {
618: (void) l;
619: (void) p;
620: /* For backwards compatibility, libnet_pblock_t no longer includes
621: an ip_offset, so calling this is unnecessary.
622: */
623: }
624:
625:
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>