Debian Codegraph @

protolib/src/common/protoPktIP.cpp

raw
1 /**
2 * @file protoPktIP.cpp
3 *
4 * @brief These classes provide representations of IP packet formats for IPv4 and IPv6 packets:
5 */
6
7 #include "protoPktIP.h"
8 #include "protoDebug.h"
9
10 // begin ProtoPktIP implementation
11 ProtoPktIP::ProtoPktIP(void* bufferPtr,
12 unsigned int numBytes,
13 bool freeOnDestruct)
14 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct)
15 {
16 }
17
18 ProtoPktIP::~ProtoPktIP()
19 {
20 }
21
22 unsigned int ProtoPktIP::GetHeaderLength()
23 {
24 switch (GetVersion())
25 {
26 case 4:
27 {
28 ProtoPktIPv4 ip4Pkt(*this);
29 return ip4Pkt.GetHeaderLength();
30 }
31 case 6:
32 {
33 // IPv6 has fixed-length header always
34 return ProtoPktIPv6::GetHeaderLength();
35 }
36 default:
37 return 0;
38 }
39 } // end ProtoPktIP::GetHeaderLength()
40
41 bool ProtoPktIP::GetDstAddr(ProtoAddress& dst)
42 {
43 switch (GetVersion())
44 {
45 case 4:
46 {
47 ProtoPktIPv4 ip4Pkt(*this);
48 ip4Pkt.GetDstAddr(dst);
49 return true;
50 }
51 case 6:
52 {
53 ProtoPktIPv6 ip6Pkt(*this);
54 ip6Pkt.GetDstAddr(dst);
55 return true;
56 }
57 default:
58 return false;
59 }
60 } // end ProtoPktIP::GetDstAddr()
61
62 bool ProtoPktIP::SetDstAddr(ProtoAddress& dst)
63 {
64 switch (GetVersion())
65 {
66 case 4:
67 {
68 ProtoPktIPv4 ip4Pkt(*this);
69 ip4Pkt.SetDstAddr(dst, true);
70 return true;
71 }
72 case 6:
73 {
74 ProtoPktIPv6 ip6Pkt(*this);
75 ip6Pkt.SetDstAddr(dst);
76 return true;
77 }
78 default:
79 return false;
80 }
81 } // end ProtoPktIP::SetDstAddr()
82
83
84 bool ProtoPktIP::GetSrcAddr(ProtoAddress& src)
85 {
86 switch (GetVersion())
87 {
88 case 4:
89 {
90 ProtoPktIPv4 ip4Pkt(*this);
91 ip4Pkt.GetSrcAddr(src);
92 return true;
93 }
94 case 6:
95 {
96 ProtoPktIPv6 ip6Pkt(*this);
97 ip6Pkt.GetSrcAddr(src);
98 return true;
99 }
100 default:
101 return false;
102 }
103 } // end ProtoPktIP::GetSrcAddr()
104
105 bool ProtoPktIP::SetSrcAddr(ProtoAddress& src)
106 {
107 switch (GetVersion())
108 {
109 case 4:
110 {
111 ProtoPktIPv4 ip4Pkt(*this);
112 ip4Pkt.SetSrcAddr(src, true);
113 return true;
114 }
115 case 6:
116 {
117 ProtoPktIPv6 ip6Pkt(*this);
118 ip6Pkt.SetSrcAddr(src);
119 return true;
120 }
121 default:
122 return false;
123 }
124 } // end ProtoPktIP::SetSrcAddr()
125
126 // begin ProtoPktIPv4 implementation
127 ProtoPktIPv4::ProtoPktIPv4(void* bufferPtr,
128 unsigned int numBytes,
129 bool initFromBuffer,
130 bool freeOnDestruct)
131 : ProtoPktIP(bufferPtr, numBytes, freeOnDestruct)
132 {
133 if (initFromBuffer)
134 InitFromBuffer();
135 else if (NULL != bufferPtr)
136 InitIntoBuffer();
137 }
138
139 ProtoPktIPv4::ProtoPktIPv4(ProtoPktIP & ipPkt)
140 : ProtoPktIP(ipPkt.AccessBuffer(), ipPkt.GetBufferLength())
141 {
142 InitFromBuffer();
143 }
144
145 ProtoPktIPv4::~ProtoPktIPv4()
146 {
147 }
148
149 bool ProtoPktIPv4::InitFromBuffer(void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
150 {
151 if (NULL != bufferPtr)
152 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
153 else
154 ProtoPkt::SetLength(0);
155 if (GetBufferLength() > OFFSET_VERSION)
156 {
157 if (4 == (GetUINT8(OFFSET_VERSION) >> 4))
158 {
159 if (GetBufferLength() > (2*OFFSET_LEN + 2) &&
160 ProtoPkt::InitFromBuffer(GetTotalLength()))
161 {
162 return true;
163 }
164 else
165 {
166 PLOG(PL_ERROR, "ProtoPktIPv4::InitFromBuffer() error: insufficient buffer space!\n");
167 }
168 }
169 else
170 {
171 PLOG(PL_WARN, "ProtoPktIPv4::InitFromBuffer() error: invalid protocol version!\n");
172 }
173 }
174 if (NULL != bufferPtr) DetachBuffer();
175 return false;
176 } // end ProtoPktIPv4::InitFromBuffer()
177
178
179 bool ProtoPktIPv4::InitIntoBuffer(void* bufferPtr, unsigned int bufferBytes, bool freeOnDestruct)
180 {
181 if (NULL != bufferPtr)
182 {
183 if (bufferBytes < 20)
184 return false;
185 else
186 AttachBuffer(bufferPtr, bufferBytes, freeOnDestruct);
187 }
188 else if (GetBufferLength() < 20)
189 {
190 return false;
191 }
192 SetVersion(4);
193 SetHeaderLength(20);
194 SetChecksum(0);
195 SetWord16(OFFSET_FRAGMENT, 0); // init flags & frag to ZERO
196 SetTOS(0);
197 SetUINT8(OFFSET_FLAGS, 0);
198 SetTTL(255);
199 SetProtocol(RESERVED);
200 // (TBD) Set some header fields to default values? (e.g. fragment, flags)
201 // (TBD) should we set total length to 20 here?
202 return true;
203 } // end ProtoPktIPv4::InitIntoBuffer()
204
205 void ProtoPktIPv4::SetTOS(UINT8 tos, bool updateChecksum)
206 {
207 if (updateChecksum)
208 {
209 bool oddOffset = (0 != (0x01 & OFFSET_TOS));
210 UpdateChecksum(GetTOS(), tos, oddOffset);
211 }
212 SetUINT8(OFFSET_TOS, tos);
213 } // end ProtoPktIPv4::SetTOS()
214
215 void ProtoPktIPv4::SetID(UINT16 id, bool updateChecksum)
216 {
217 if (updateChecksum) UpdateChecksum(GetID(), id);
218 SetWord16(OFFSET_ID, id);
219 } // end ProtoPktIPv4::SetID()
220
221 void ProtoPktIPv4::SetFlag(Flag flag, bool updateChecksum)
222 {
223 if (updateChecksum)
224 {
225 bool oddOffset = (0 != (0x01 & OFFSET_FLAGS));
226 UINT8 oldFlags = GetUINT8(OFFSET_FLAGS);
227 UINT8 newFlags = oldFlags | (UINT8)flag;
228 SetUINT8(OFFSET_FLAGS, newFlags);
229 UpdateChecksum(oldFlags, newFlags, oddOffset);
230 }
231 else
232 {
233 AccessUINT8(OFFSET_FLAGS) |= flag;
234 }
235 } // end ProtoPktIPv4::SetFlag()
236
237 void ProtoPktIPv4::ClearFlag(Flag flag, bool updateChecksum)
238 {
239 if (updateChecksum)
240 {
241 bool oddOffset = (0 != (0x01 & OFFSET_FLAGS));
242 UINT8 oldFlags = GetUINT8(OFFSET_FLAGS);
243 UINT8 newFlags = oldFlags & ~(UINT8)flag;
244 SetUINT8(OFFSET_FLAGS, newFlags);
245 UpdateChecksum(oldFlags, newFlags, oddOffset);
246 }
247 else
248 {
249 AccessUINT8(OFFSET_FLAGS) &= ~flag;
250 }
251 } // end ProtoPktIPv4::ClearFlag()
252
253 void ProtoPktIPv4::SetFragmentOffset(UINT16 fragmentOffset, bool updateChecksum)
254 {
255 UINT16 fragOld = GetWord16(OFFSET_FRAGMENT);
256 UINT16 fragNew = fragOld & 0xe000;
257 fragNew |= (fragmentOffset & 0x1fff);
258 if (updateChecksum) UpdateChecksum(fragOld, fragNew);
259 SetWord16(OFFSET_FRAGMENT, fragNew);
260 } // end ProtoPktIPv4::SetFragmentOffset()
261
262
263 void ProtoPktIPv4::SetTTL(UINT8 ttl, bool updateChecksum)
264 {
265 if (updateChecksum)
266 {
267 bool oddOffset = (0 != (0x01 & OFFSET_TTL));
268 UpdateChecksum(GetTTL(), ttl, oddOffset);
269 }
270 SetUINT8(OFFSET_TTL, ttl);
271 } // end ProtoPktIPv4::SetTTL()
272
273 void ProtoPktIPv4::SetProtocol(Protocol protocol, bool updateChecksum)
274 {
275 if (updateChecksum)
276 {
277 bool oddOffset = (0 != (0x01 & OFFSET_PROTOCOL));
278 UpdateChecksum((UINT8)GetProtocol(), (UINT8)protocol, oddOffset);
279 }
280 SetUINT8(OFFSET_PROTOCOL, (UINT8)protocol);
281 } // end ProtoPktIPv4::SetProtocol()
282
283
284 void ProtoPktIPv4::SetSrcAddr(const ProtoAddress& addr, bool calculateChecksum)
285 {
286 memcpy((char*)AccessBuffer32(OFFSET_SRC_ADDR), addr.GetRawHostAddress(), 4);
287 if (calculateChecksum) CalculateChecksum(); // (TBD) is it worth it to incrementally update
288 } // end ProtoPktIPv4::SetSrcAddr()
289
290 void ProtoPktIPv4::SetDstAddr(const ProtoAddress& addr, bool calculateChecksum)
291 {
292 memcpy((char*)AccessBuffer32(OFFSET_DST_ADDR), addr.GetRawHostAddress(), 4);
293 if (calculateChecksum) CalculateChecksum(); // (TBD) is it worth it to incrementally update
294 } // end ProtoPktIPv4::SetDstAddr()
295
296 void ProtoPktIPv4::SetPayloadLength(UINT16 numBytes, bool calculateChecksum)
297 {
298 SetTotalLength(numBytes + GetHeaderLength());
299 if (calculateChecksum) CalculateChecksum();
300 } // end ProtoPktIPv4::SetPayloadLength()
301
302 /**
303 * Return checksum in host byte order
304 */
305 UINT16 ProtoPktIPv4::CalculateChecksum(bool set)
306 {
307 UINT32 sum = 0;
308 // Calculate checksum, skipping checksum field
309 unsigned int i;
310 for (i = 0; i < OFFSET_CHECKSUM; i++)
311 sum += GetWord16(i);
312 unsigned int hdrEndex = (GetUINT8(OFFSET_HDR_LEN) & 0x0f) << 1;
313 for (i = OFFSET_CHECKSUM+1; i < hdrEndex; i++)
314 sum += GetWord16(i);
315 while (sum >> 16)
316 sum = (sum & 0x0000ffff) + (sum >> 16);
317 sum = ~sum;
318 if (set) SetChecksum(sum);
319 return sum;
320 } // ProtoPktIPv4::CalculateChecksum()
321
322
323 // begin ProtoPktIPv4::Option implementation
324
325 ProtoPktIPv4::Option::Option(void* bufferPtr, unsigned int numBytes, bool initFromBuffer, bool freeOnDestruct)
326 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct)
327 {
328 if (initFromBuffer && (NULL != bufferPtr)) InitFromBuffer();
329 }
330
331 ProtoPktIPv4::Option::~Option()
332 {
333 }
334
335 int ProtoPktIPv4::Option::GetLengthByType(Type type)
336 {
337 switch (type)
338 {
339 // fixed length options we support
340 case EOOL:
341 case NOP:
342 return 1;
343 case SEC:
344 return 11;
345 case SID:
346 case MTUP:
347 case MTUR:
348 case RTRALT:
349 return 4;
350 // variable length options we support
351 case LSR:
352 case SSR:
353 case RR:
354 case ESEC:
355 case TS:
356 case TR:
357 case SDB:
358 case EIP:
359 case CIPSO:
360 return LENGTH_VARIABLE; // indicates variable length
361 case UMP:
362 return 8;
363 // unsupported options
364 default:
365 return LENGTH_UNKNOWN; // unsupported type
366 }
367 } // end ProtoPktIPv4::Option::GetLengthByType()
368
369 bool ProtoPktIPv4::Option::IsMutable(Type type)
370 {
371 // per RFC4302
372 switch (type)
373 {
374 // Immutable options
375 case EOOL:
376 case NOP:
377 case SEC:
378 case ESEC:
379 case CIPSO:
380 case RTRALT:
381 case SDB:
382 return false;
383
384 // Assume others are mutable
385 default:
386 return true;
387 }
388 } // end ProtoPktIPv4::Option::IsMutable()
389
390
391 bool ProtoPktIPv4::Option::InitFromBuffer(void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
392 {
393 if (NULL != bufferPtr) AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
394 if (GetBufferLength() > OFFSET_TYPE)
395 {
396 // Validate buffer length according to type
397 Type type = (Type)GetUINT8(OFFSET_TYPE);
398 int minLength = GetLengthByType(type);
399 switch (minLength)
400 {
401 case LENGTH_UNKNOWN:
402 PLOG(PL_ERROR, "ProtoPktIPv4::Option::InitFromBuffer() error: unsupported type: %d\n", (UINT8)type);
403 return false;
404 case LENGTH_VARIABLE:
405 if (GetBufferLength() <= OFFSET_LENGTH)
406 {
407 PLOG(PL_ERROR, "ProtoPktIPv4::Option::InitFromBuffer() error: incomplete buffer\n");
408 return false;
409 }
410 minLength = (int)GetUINT8(OFFSET_LENGTH);
411 break;
412 default:
413 break;
414 }
415 if (GetBufferLength() < (unsigned int)minLength)
416 {
417 PLOG(PL_ERROR, "ProtoPktIPv4::Option::InitFromBuffer() error: incomplete buffer\n");
418 return false;
419 }
420 else
421 {
422 SetLength(minLength);
423 return true;
424 }
425 }
426 else
427 {
428 PLOG(PL_ERROR, "ProtoPktIPv4::Option::InitFromBuffer() error: insufficient buffer\n");
429 return false;
430 }
431 } // end ProtoPktIPv4::Option::InitFromBuffer()
432
433
434 bool ProtoPktIPv4::Option::InitIntoBuffer(Type type,
435 void* bufferPtr,
436 unsigned int numBytes,
437 bool freeOnDestruct)
438 {
439 int minLength = GetLengthByType(type);
440 switch (minLength)
441 {
442 case LENGTH_UNKNOWN:
443 PLOG(PL_ERROR, "ProtoPktIPv4::Option::InitIntoBuffer() error: unsupported type: %d\n", type);
444 return false;
445 case LENGTH_VARIABLE:
446 minLength = 2; // for type + length fields
447 break;
448 default:
449 break;
450 }
451 if (NULL != bufferPtr)
452 {
453 if (numBytes < (unsigned int)minLength)
454 return false;
455 else
456 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
457 }
458 else if (GetBufferLength() < (unsigned int)minLength)
459 {
460 return false;
461 }
462 // zero init "minLength" bytes?
463 SetUINT8(OFFSET_TYPE, (UINT8)type);
464 SetUINT8(OFFSET_LENGTH, (UINT8)minLength);
465 SetLength(minLength);
466 // Zero the remaining content
467 if (minLength > OFFSET_LENGTH + 1)
468 memset(AccessBuffer(OFFSET_LENGTH + 1), 0, minLength - (OFFSET_LENGTH+1));
469 return true;
470 } // end ProtoPktIPv4::Option::InitIntoBuffer()
471
472 bool ProtoPktIPv4::Option::SetData(const char* data, unsigned int length)
473 {
474 if (0 == GetBufferLength())
475 {
476 PLOG(PL_ERROR, "ProtoPktIPv4::Option::SetData() error: no buffer attached\n");
477 return false;
478 }
479 // 1) Make sure data will fit into buffer _and_ appropo for type
480 int optLength = GetLengthByType(GetType());
481 int maxLength = 0;
482 switch (optLength)
483 {
484 case LENGTH_UNKNOWN:
485 PLOG(PL_ERROR, "ProtoPktIPv4::Option::SetData() error: unsupported type: %d\n", GetType());
486 return false;
487
488 case LENGTH_VARIABLE:
489 optLength = length + 2;
490 maxLength = (GetBufferLength() < 2) ? 0 : (GetBufferLength() - 2);
491 break;
492
493 default:
494 maxLength = (GetBufferLength() < (unsigned int)optLength) ? 0 : optLength - 2;
495 break;
496 }
497 if (length > (unsigned int)maxLength)
498 {
499 PLOG(PL_ERROR, "ProtoPktIPv4::Option::SetData() error: insufficient buffer space\n");
500 return false;
501 }
502 SetUINT8(OFFSET_LENGTH, optLength);
503 SetLength(optLength);
504 memcpy((char*)AccessBuffer(2), data, length);
505 return true;
506 } // end ProtoPktIPv4::Option::SetData()
507
508 ProtoPktIPv4::Option::Iterator::Iterator(const ProtoPktIPv4& ip4Pkt)
509 : pkt_buffer((const char*)ip4Pkt.GetBuffer()), offset(20)
510 {
511 if (ip4Pkt.GetLength() > 20)
512 {
513 unsigned int hlen = ip4Pkt.GetHeaderLength();
514 if (hlen > 20)
515 offset_end = hlen;
516 else
517 offset_end = 20; // no options
518 }
519 else
520 {
521 offset_end = 20;
522 }
523 }
524
525 ProtoPktIPv4::Option::Iterator::~Iterator()
526 {
527 }
528
529 bool ProtoPktIPv4::Option::Iterator::GetNextOption(Option& option)
530 {
531 if (offset < offset_end)
532 {
533 bool result = option.InitFromBuffer((char*)pkt_buffer + offset, offset_end - offset);
534 offset = result ? (offset + option.GetLength()) : offset_end;
535 return result;
536 }
537 else
538 {
539 return false;
540 }
541 } // end ProtoPktIPv4::Option::Iterator::GetNextOption()
542
543
544 // begin ProtoPktIPv6 implementation
545
546 ProtoPktIPv6::ProtoPktIPv6(void* bufferPtr,
547 unsigned int numBytes,
548 bool initFromBuffer,
549 bool freeOnDestruct)
550 : ProtoPktIP(bufferPtr, numBytes, freeOnDestruct)
551 {
552 if (initFromBuffer)
553 InitFromBuffer();
554 else if (NULL != bufferPtr)
555 InitIntoBuffer();
556 }
557
558 ProtoPktIPv6::ProtoPktIPv6(ProtoPktIP & ipPkt)
559 : ProtoPktIP(ipPkt.AccessBuffer(), ipPkt.GetBufferLength()),
560 ext_pending(false)
561 {
562 InitFromBuffer();
563 }
564
565 ProtoPktIPv6::~ProtoPktIPv6()
566 {
567 }
568
569 bool ProtoPktIPv6::InitFromBuffer(void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
570 {
571 ext_pending = false;
572 if (NULL != bufferPtr)
573 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
574 else
575 ProtoPkt::SetLength(0);
576 if (GetBufferLength() > OFFSET_VERSION)
577 {
578 if (6 != (GetUINT8(OFFSET_VERSION) >> 4))
579 {
580 PLOG(PL_ERROR, "ProtoPktIPv6::InitFromBuffer() error: invalid version number\n");
581 }
582 else if (GetBufferLength() > (2*OFFSET_LENGTH + 2))
583 {
584 // this validates that the embedded length is <= buffer_bytes
585 if (ProtoPkt::InitFromBuffer(40 + GetPayloadLength()))
586 {
587 return true;
588 }
589 else
590 {
591 PLOG(PL_ERROR, "ProtoPktIPv6::InitFromBuffer() error: invalid packet length?\n");
592 }
593 }
594 else
595 {
596 PLOG(PL_ERROR, "ProtoPktIPv6::InitFromBuffer() error: insufficient buffer space (2)\n");
597 }
598 }
599 else
600 {
601 PLOG(PL_ERROR, "ProtoPktIPv6::InitFromBuffer() error: insufficient buffer space (1)\n");
602 }
603 if (NULL != bufferPtr) DetachBuffer();
604 return false;
605 } // end ProtoPktIPv6::InitFromBuffer()
606
607
608 bool ProtoPktIPv6::InitIntoBuffer(void* bufferPtr,
609 unsigned int bufferBytes,
610 bool freeOnDestruct)
611 {
612 if (NULL != bufferPtr)
613 {
614 if (bufferBytes < 40)
615 return false;
616 else
617 AttachBuffer(bufferPtr, bufferBytes, freeOnDestruct);
618 }
619 else if (GetBufferLength() < 40)
620 {
621 return false;
622 }
623 SetVersion(6);
624 SetTrafficClass(0);
625 SetFlowLabel(0);
626 SetPayloadLength(0);
627 SetNextHeader(NONE);
628 SetLength(40);
629 ext_pending = false;
630 return true;
631 } // end ProtoPktIPv6::InitIntoBuffer()
632
633 ProtoPktIP::Protocol ProtoPktIPv6::GetLastHeader()
634 {
635 if (HasExtendedHeader())
636 {
637 Extension::Iterator iterator(*this);
638 Extension lastExt;
639 while (iterator.GetNextExtension(lastExt));
640 return lastExt.GetNextHeader();
641 }
642 else
643 {
644 return GetNextHeader();
645 }
646 } // end ProtoPktIPv6::GetLastHeader()
647
648 /**
649 * Map extension to end of current IPv6 packet header
650 */
651 ProtoPktIPv6::Extension* ProtoPktIPv6::AddExtension(Protocol extensionType)
652 {
653 if (NONE == extensionType) return NULL;
654 unsigned int hdrBytes = 40;
655 if (ext_pending)
656 {
657 PackHeader(extensionType);
658 hdrBytes = ProtoPkt::GetLength();
659 }
660 else if (HasExtendedHeader())
661 {
662 Extension::Iterator iterator(*this);
663 Extension lastExt, x;
664 while (iterator.GetNextExtension(x))
665 {
666 hdrBytes += x.GetLength();
667 lastExt = x;
668 }
669 lastExt.SetNextHeader(extensionType);
670 }
671 else
672 {
673 if (GetBufferLength() <= hdrBytes) return NULL;
674 // (TBD) Should we return NULL if (NONE != GetNextHeader())???
675 ASSERT(NONE == GetNextHeader());
676 SetNextHeader(extensionType);
677 }
678 ASSERT(0 == (hdrBytes & 0x03));
679 ext_temp.AttachBuffer(AccessBuffer32(hdrBytes >> 2), GetBufferLength() - hdrBytes);
680 ext_temp.SetType(extensionType);
681 ext_pending = true;
682 return &ext_temp;
683 } // end ProtoPktIPv6::AddExtension()
684
685 bool ProtoPktIPv6::PackHeader(Protocol nextHeaderType)
686 {
687 if (ext_pending)
688 {
689 if (!ext_temp.Pack()) return false;
690 ext_temp.SetNextHeader(nextHeaderType);
691 // Update IPv6 packet payload length field
692 SetPayloadLength(GetPayloadLength() + ext_temp.GetLength());
693 ext_pending = false;
694 }
695 else if (HasExtendedHeader())
696 {
697 // (TBD) Find last header extension and set next header type?
698 }
699 else
700 {
701 SetNextHeader(nextHeaderType);
702 }
703 return true;
704 } // end ProtoPktIPv6::PackHeader()
705
706 /**
707 * Copy extension to beginning of packet's set of header
708 * extensions (payload is moved if present)
709 */
710 bool ProtoPktIPv6::PrependExtension(Extension& ext)
711 {
712 if (ext_pending) PackHeader();
713 // 1) Is there room in the IPv6 packer buffer_ptr for the extension?
714 if (GetBufferLength() < (ProtoPkt::GetLength() + ext.GetLength())) return false;
715 // 2) Copy current packet next header to our ext next header
716 ext.SetNextHeader(GetNextHeader());
717 // 3) Move the current payload to make room for the extension
718 UINT16 payloadLength = GetPayloadLength();
719 void* ptr = AccessBuffer(40);
720 memmove((char*)ptr+ext.GetLength(), ptr, payloadLength);
721 // 4) Copy extension buffer_ptr into space made available
722 memcpy(ptr, (char*)ext.GetBuffer(), ext.GetLength());
723 SetNextHeader(ext.GetType());
724 SetPayloadLength(payloadLength + ext.GetLength());
725 return true;
726 } // end ProtoPktIPv6::PrependExtension()
727
728
729 bool ProtoPktIPv6::ReplaceExtension(Extension& oldExt, Extension& newExt)
730 {
731 ASSERT((oldExt.GetBuffer() >= ((char*)GetBuffer() + 40)) &&
732 (oldExt.GetBuffer() < ((char*)GetBuffer() + GetLength())));
733
734 if (oldExt.GetType() == newExt.GetType())
735 {
736 int spaceAvailable = GetBufferLength() - GetLength();
737 int spaceDelta = newExt.GetLength() - oldExt.GetLength();
738 if (spaceDelta > spaceAvailable)
739 {
740 PLOG(PL_ERROR, "ProtoPktIPv6::ReplaceExtension() error: insufficient buffer space!\n");
741 return false;
742 }
743 newExt.SetNextHeader(oldExt.GetNextHeader());
744 char* dataPtr = (char*)oldExt.AccessBuffer() + oldExt.GetLength();
745 UINT16 dataLen = (UINT16)((char*)GetBuffer() + GetLength() - dataPtr);
746 memmove(dataPtr+spaceDelta, dataPtr, dataLen);
747 memcpy((char*)oldExt.AccessBuffer(), (char*)newExt.GetBuffer(), newExt.GetLength());
748 SetPayloadLength(GetPayloadLength() + spaceDelta);
749 return true;
750 }
751 else
752 {
753 // (TBD) we could mod to allow a new type of extension to replace the old one
754 PLOG(PL_ERROR, "ProtoPktIPv6::ReplaceExtension() error: new extension is of different type!\n");
755 return false;
756 }
757 } // end ProtoPktIPv6::ReplaceExtension()
758
759 /**
760 * Copy pre-built extension to end of current IPv6 header
761 * (payload is moved if present)
762 */
763 bool ProtoPktIPv6::AppendExtension(Extension& ext)
764 {
765 unsigned int hdrBytes = 40;
766 if (ext_pending)
767 {
768
769 PackHeader(ext.GetType());
770 // Is there room in the IPv6 packer buffer_ptr for the additional extension?
771 if (GetBufferLength() < (ProtoPkt::GetLength() + ext.GetLength())) return false;
772 hdrBytes = ProtoPkt::GetLength();
773 }
774 else if (HasExtendedHeader())
775 {
776 // Is there room in the IPv6 packer buffer_ptr for the additional extension?
777 if (GetBufferLength() < (ProtoPkt::GetLength() + ext.GetLength())) return false;
778 Extension::Iterator iterator(*this);
779 Extension lastExt, x;
780 while (iterator.GetNextExtension(x))
781 {
782 hdrBytes += x.GetLength();
783 lastExt = x;
784 }
785 ext.SetNextHeader(lastExt.GetNextHeader());
786 lastExt.SetNextHeader(ext.GetType());
787 }
788 else
789 {
790 // Is there room in the IPv6 packer buffer_ptr for the additional extension?
791 if (GetBufferLength() < (ProtoPkt::GetLength() + ext.GetLength())) return false;
792 ext.SetNextHeader(GetNextHeader());
793 SetNextHeader(ext.GetType());
794 }
795 void* ptr = AccessBuffer(hdrBytes);
796 UINT16 payloadLength = GetPayloadLength();
797 UINT16 moveLength = payloadLength + 40 - hdrBytes;
798 memmove((char*)ptr + ext.GetLength(), ptr, moveLength);
799 memcpy(ptr, (char*)ext.GetBuffer(), ext.GetLength());
800 SetPayloadLength(payloadLength + ext.GetLength());
801 return true;
802 } // end ProtoPktIPv6::AppendExtension()
803
804 bool ProtoPktIPv6::SetPayload(Protocol payloadType, const char* dataPtr, UINT16 dataLen)
805 {
806 if (ext_pending)
807 {
808 // Is there room for the payload?
809 if (GetBufferLength() < (ProtoPkt::GetLength() + ext_temp.GetLength() + dataLen)) return false;
810 PackHeader(payloadType);
811 }
812 else if (HasExtendedHeader())
813 {
814 // Is there room for the payload?
815 if (GetBufferLength() < (ProtoPkt::GetLength() + dataLen))
816 {
817 PLOG(PL_ERROR, "ProtoPktIPv6::SetPayload() error: insufficient buffer space (1)\n");
818 return false;
819 }
820 ASSERT(GetPayloadLength() > 0);
821 // Find last extension header
822 Extension::Iterator iterator(*this);
823 Extension lastExt, x;
824 while (iterator.GetNextExtension(x))
825 lastExt = x;
826 ASSERT(NONE == lastExt.GetNextHeader());
827 lastExt.SetNextHeader(payloadType);
828 }
829 else
830 {
831 // Is there already a payload set?
832 ASSERT(NONE == GetNextHeader());
833 // Is there room for the payload?
834 if (GetBufferLength() < (ProtoPkt::GetLength() + dataLen))
835 {
836 PLOG(PL_ERROR, "ProtoPktIPv6::SetPayload() error: insufficient buffer space (2)\n");
837 return false;
838 }
839 ASSERT(0 == GetPayloadLength());
840 SetNextHeader(payloadType);
841 }
842 // 3) Copy payload into buffer
843 memcpy(AccessBuffer(ProtoPkt::GetLength()), dataPtr, dataLen);
844 SetPayloadLength(GetPayloadLength() + dataLen);
845 return true;
846 } // end ProtoPktIPv6::SetPayload()
847
848
849 ProtoPktIPv6::Extension::Extension(Protocol extType,
850 void* bufferPtr,
851 unsigned int numBytes,
852 bool initFromBuffer,
853 bool freeOnDestruct)
854 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct), ext_type(NONE), opt_pending(false), opt_packed(false)
855 {
856 if (initFromBuffer)
857 InitFromBuffer(extType);
858 else
859 InitIntoBuffer(extType);
860 }
861
862 ProtoPktIPv6::Extension::~Extension()
863 {
864 }
865
866 bool ProtoPktIPv6::Extension::Copy(const ProtoPktIPv6::Extension& ext)
867 {
868 if (ext.GetLength() > GetBufferLength())
869 {
870 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::Copy() error: insufficient buffer size\n");
871 return false;
872 }
873 // a) save our buffer pointer, length, and allocation status
874 UINT32 bufferBytes = GetBufferLength();
875 bool freeOnDestruct = FreeOnDestruct();
876 UINT32* bufferPtr = DetachBuffer();
877 // b) copy all 'ext' member variable values
878 *this = ext;
879 // c) restore buffer pointer, buffer length, and set to 'ext' length
880 DetachBuffer(); // detach the 'ext' buffer pointers copied
881 ProtoPkt::InitFromBuffer(ext.GetLength(), bufferPtr, bufferBytes, freeOnDestruct);
882 // d) copy 'ext' buffer content
883 memcpy(bufferPtr, ext.GetBuffer(), ext.GetLength());
884 return true;
885 } // end ProtoPktIPv6::Extension::Copy()
886
887 bool ProtoPktIPv6::Extension::InitIntoBuffer(Protocol extType,
888 void* bufferPtr,
889 unsigned int numBytes,
890 bool freeOnDestruct)
891 {
892 if (NULL != bufferPtr) AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
893 ext_type = extType;
894 if (0 == GetBufferLength())
895 return true; // don't worry if no buffer_ptr for moment
896 else if (GetBufferLength() > OFFSET_NEXT_HDR)
897 SetNextHeader(NONE);
898 switch (extType)
899 {
900 // These types have length fields
901 default:
902 PLOG(PL_WARN, "ProtoPktIPv6::Extension::InitIntoBuffer() warning: unknown extension type\n");
903 // For now we'll assume it has a length field ala the options and routing extensions
904 case NONE:
905 case HOPOPT:
906 case DSTOPT:
907 case RTG:
908 case AUTH:
909 if (GetBufferLength() > OFFSET_LENGTH)
910 {
911 ProtoPkt::SetLength(2);
912 break;
913 }
914 else
915 {
916 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::InitIntoBuffer() error: insufficient buffer space\n");
917 SetLength(0);
918 if (NULL != bufferPtr)
919 DetachBuffer();
920 return false;
921 }
922 case FRAG:
923 if (GetBufferLength() > OFFSET_LENGTH) SetExtensionLength(8);
924 break;
925 }
926 opt_pending = opt_packed = false;
927 return true;
928 } // ProtoPktIPv6::Extension::InitIntoBuffer()
929
930 /**
931 * Set length field (as applicable) of extension header from units of bytes
932 */
933 void ProtoPktIPv6::Extension::SetExtensionLength(UINT16 numBytes)
934 {
935 switch (GetType())
936 {
937 default:
938 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::SetExtensionLength() unknown extension type\n");
939 // For now we'll assume it has a length field ala the options and routing extensions
940 case HOPOPT:
941 case DSTOPT:
942 case RTG:
943 ASSERT(0 == (0x07 & numBytes));
944 SetUINT8(OFFSET_LENGTH, (numBytes - 8) >> 3);
945 break;
946 case AUTH:
947 ASSERT(0 == (0x03 & numBytes));
948 SetUINT8(OFFSET_LENGTH, (numBytes - 4) >> 2);
949 case FRAG:
950 ASSERT(8 == numBytes);
951 break;
952 }
953 ProtoPkt::SetLength(numBytes);
954 } // end ProtoPktIPv6::Extension::SetExtensionLength()
955
956 ProtoPktIPv6::Option* ProtoPktIPv6::Extension::AddOption(Option::Type optType)
957 {
958 if (opt_packed)
959 {
960 ASSERT(!opt_pending);
961 // Find first PADx option and truncate extension length
962 Option::Iterator iterator(*this);
963 Option opt;
964 while (iterator.GetNextOption(opt))
965 {
966 Option::Type otype = opt.GetType();
967 if ((Option::PAD1 == otype) || (Option::PADN == otype))
968 {
969 size_t extLen = (char*)opt.GetBuffer() - (char*)GetBuffer();
970 if (extLen != (ProtoPkt::GetLength() - opt.GetLength()))
971 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::AddOption() warning: extension used multiple PADS ?!\n");
972 ProtoPkt::SetLength((unsigned int)extLen);
973 break;
974 }
975 }
976 opt_packed = false;
977 }
978 else if (opt_pending)
979 {
980 PackOption();
981 }
982 // If there is any room in the buffer, init the new option into the available space
983 unsigned int minLength = (Option::PAD1 == optType) ? 1 : 2;
984 unsigned int bufferSpace = GetBufferLength() - ProtoPkt::GetLength();
985 if (bufferSpace < minLength)
986 {
987 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::AddOption() error: insufficient buffer space\n");
988 return NULL;
989 }
990 opt_temp.InitIntoBuffer(optType, (char*)AccessBuffer(ProtoPkt::GetLength()), bufferSpace);
991 if (Option::PAD1 != optType) opt_temp.SetData(NULL, 0);
992 opt_pending = true;
993 return &opt_temp;
994 } // end ProtoPktIPv6::Extension::AddOption()
995
996 bool ProtoPktIPv6::Extension::ReplaceOption(Option& oldOpt, Option& newOpt)
997 {
998 ASSERT((oldOpt.GetBuffer() >= ((char*)GetBuffer() + 2)) &&
999 (oldOpt.GetBuffer() < ((char*)GetBuffer() + GetLength())));
1000 if (opt_packed)
1001 {
1002 ASSERT(!opt_pending);
1003 // Find first PADx option and truncate extension length
1004 Option::Iterator iterator(*this);
1005 Option opt;
1006 while (iterator.GetNextOption(opt))
1007 {
1008 Option::Type otype = opt.GetType();
1009 if ((Option::PAD1 == otype) || (Option::PADN == otype))
1010 {
1011 size_t extLen = (char*)opt.GetBuffer() - (char*)GetBuffer();
1012 if (extLen != (ProtoPkt::GetLength() - opt.GetLength()))
1013 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::AddOption() warning: extension used multiple PADS ?!\n");
1014 ProtoPkt::SetLength((unsigned int)extLen);
1015 break;
1016 }
1017 }
1018 opt_packed = false;
1019 }
1020 else if (opt_pending)
1021 {
1022 PackOption();
1023 }
1024 int spaceAvailable = GetBufferLength() - GetLength();
1025 int spaceDelta = newOpt.GetLength() - oldOpt.GetLength();
1026 if (spaceDelta > spaceAvailable)
1027 {
1028 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::ReplaceOption() error: insufficient buffer space!\n");
1029 return false;
1030 }
1031 char* dataPtr = (char*)oldOpt.AccessBuffer() + oldOpt.GetLength();
1032 UINT16 dataLen = (UINT16)((char*)GetBuffer() + GetLength() - dataPtr);
1033 memmove(dataPtr+spaceDelta, dataPtr, dataLen);
1034 memcpy(oldOpt.AccessBuffer(), newOpt.GetBuffer(), newOpt.GetLength());
1035 ProtoPkt::SetLength(ProtoPkt::GetLength() + spaceDelta);
1036 return Pack();
1037 } // end ProtoPktIPv6::Extension::ReplaceOption()
1038
1039 bool ProtoPktIPv6::Extension::Pack()
1040 {
1041 if (IsOptionHeader())
1042 {
1043 if (opt_pending) PackOption();
1044 if (PadOptionHeader())
1045 {
1046 opt_packed = true;
1047 SetExtensionLength(ProtoPkt::GetLength());
1048 return true;
1049 }
1050 else
1051 {
1052 return false;
1053 }
1054 }
1055 else
1056 {
1057 opt_packed = true;
1058 SetExtensionLength(ProtoPkt::GetLength());
1059 return true;
1060 }
1061 } // end ProtoPktIPv6::Extension::Pack()
1062
1063 void ProtoPktIPv6::Extension::PackOption()
1064 {
1065 if (opt_pending)
1066 {
1067 // Update Extension payload length field
1068 //SetExtensionLength(pkt_length + opt_temp.GetLength());
1069 ProtoPkt::SetLength(ProtoPkt::GetLength() + opt_temp.GetLength());
1070 opt_pending = false;
1071 }
1072 } // end ProtoPktIPv6::Extension::PackOption()
1073
1074 bool ProtoPktIPv6::Extension::PadOptionHeader()
1075 {
1076 PackOption();
1077 UINT8 padBytes = (UINT8)(GetLength() & 0x07);
1078 if (0 != padBytes)
1079 padBytes = 8 - padBytes;
1080 else
1081 return true; // no padding was needed
1082 Option* opt = AddOption((1 == padBytes) ? Option::PAD1 : Option::PADN);
1083 if ((NULL != opt) && opt->MakePad(padBytes))
1084 {
1085 PackOption();
1086 return true;
1087 }
1088 else
1089 {
1090 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::PadOptionHeader() error: insufficient buffer space\n");
1091 return false;
1092 }
1093 } // end ProtoPktIPv6::Extension::PadOptionHeader()
1094
1095
1096 bool ProtoPktIPv6::Extension::InitFromBuffer(Protocol extType, void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
1097 {
1098 if (NULL != bufferPtr)
1099 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1100 if (GetBufferLength() >= OFFSET_LENGTH)
1101 {
1102 // set ProtoPkt "pkt_length" and check that it is bounds of buffer_ptr
1103 ext_type = extType;
1104 opt_pending = false;
1105 opt_packed = true;
1106 return ProtoPkt::InitFromBuffer(GetExtensionLength());
1107 }
1108 else
1109 {
1110 if (NULL != bufferPtr)
1111 DetachBuffer();
1112 ProtoPkt::SetLength(0);
1113 return false;
1114 }
1115 } // end ProtoPktIPv6::Extension::InitFromBuffer()
1116
1117 /**
1118 * Return length of extension header in bytes
1119 */
1120 UINT16 ProtoPktIPv6::Extension::GetExtensionLength() const
1121 {
1122 switch (GetType())
1123 {
1124 default:
1125 PLOG(PL_ERROR, "ProtoPktIPv6::Extension::GetExtensionLength() unknown extension type\n");
1126 // For now we'll assume it has a length field ala the options and routing extensions
1127 case HOPOPT:
1128 case DSTOPT:
1129 case RTG:
1130 return (8 + (GetUINT8(OFFSET_LENGTH) << 3));
1131 case AUTH:
1132 return (4 + (GetUINT8(OFFSET_LENGTH) << 2));
1133 case FRAG:
1134 return 8;
1135 }
1136 } // end ProtoPktIPv6::Extension::GetExtensionLength()
1137
1138 // class ProtoPktIPv6::Extension::Iterator implementation
1139
1140 ProtoPktIPv6::Extension::Iterator::Iterator(ProtoPktIPv6& pkt)
1141 : ipv6_pkt(pkt), next_hdr(pkt.GetNextHeader()), offset(40)
1142 {
1143 }
1144
1145 ProtoPktIPv6::Extension::Iterator::~Iterator()
1146 {
1147 }
1148
1149 bool ProtoPktIPv6::Extension::Iterator::GetNextExtension(Extension& extension)
1150 {
1151 if ((6 != ipv6_pkt.GetVersion()) || (offset >= ipv6_pkt.GetLength())) return false;
1152 if (IsExtension(next_hdr))
1153 {
1154 if (extension.InitFromBuffer(next_hdr, ipv6_pkt.AccessBuffer32(offset >> 2), ipv6_pkt.GetLength()-offset))
1155 {
1156 next_hdr = extension.GetNextHeader();
1157 offset += extension.GetLength();
1158 return true;
1159 }
1160 else
1161 {
1162 return false;
1163 }
1164 }
1165 else
1166 {
1167 return false;
1168 }
1169 } // end ProtoPktIPv6::Extension::GetNextExtension()
1170
1171 ProtoPktIPv6::Option::Option(void* bufferPtr, unsigned int numBytes, bool initFromBuffer, bool freeOnDestruct)
1172 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct)
1173 {
1174 if (initFromBuffer && (NULL != bufferPtr)) InitFromBuffer();
1175 }
1176
1177 ProtoPktIPv6::Option::~Option()
1178 {
1179 }
1180
1181 bool ProtoPktIPv6::Option::InitFromBuffer(void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
1182 {
1183 if (NULL != bufferPtr) AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1184 if (GetBufferLength() > 0)
1185 {
1186 if (PAD1 == GetType())
1187 {
1188 return true;
1189 }
1190 else if (GetBufferLength() > 1)
1191 {
1192 // check that the embedded option length is in bounds
1193 if ((unsigned int)(OFFSET_DATA + GetDataLength()) > numBytes)
1194 {
1195 return false;
1196 }
1197 else
1198 {
1199 return true;
1200 }
1201 }
1202 else
1203 {
1204 return false;
1205 }
1206 }
1207 else
1208 {
1209 return false;
1210 }
1211 } // end ProtoPktIPv6::Option::InitFromBuffer()
1212
1213 bool ProtoPktIPv6::Option::InitIntoBuffer(Type type,
1214 void* bufferPtr,
1215 unsigned int numBytes,
1216 bool freeOnDestruct)
1217 {
1218 UINT8 minLen = (type == PAD1) ? 1 : 2;
1219 if (NULL != bufferPtr)
1220 {
1221 if (numBytes < minLen)
1222 return false;
1223 else
1224 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1225 }
1226 else if (GetBufferLength() < minLen)
1227 {
1228 return false;
1229 }
1230 SetUnknownPolicy(SKIP);
1231 SetMutable(false);
1232 SetType(type);
1233 if (PAD1 != type) SetDataLength(0);
1234 return true;
1235 } // end ProtoPktIPv6::Option::InitIntoBuffer()
1236
1237 bool ProtoPktIPv6::Option::SetData(char* dataPtr, UINT8 dataLen)
1238 {
1239 if (dataLen < (GetBufferLength() - OFFSET_DATA))
1240 {
1241 memcpy(AccessBuffer(OFFSET_DATA), dataPtr, dataLen);
1242 SetDataLength(dataLen);
1243 return true;
1244 }
1245 else
1246 {
1247 return false;
1248 }
1249 } // end ProtoPktIPv6::Option::SetData()
1250
1251 bool ProtoPktIPv6::Option::MakePad(UINT8 numBytes)
1252 {
1253 if (GetBufferLength() > OFFSET_TYPE)
1254 {
1255 if (numBytes > 1)
1256 {
1257 if (GetBufferLength() > (unsigned int)(OFFSET_TYPE + numBytes))
1258 {
1259 SetType(PADN);
1260 memset(AccessBuffer(OFFSET_DATA), 0, numBytes-2);
1261 SetDataLength(numBytes - (OFFSET_TYPE+2));
1262 return true;
1263 }
1264 else
1265 {
1266 PLOG(PL_ERROR, "ProtoPktIPv6::Option::MakePad() error: insufficient buffer space\n");
1267 return false;
1268 }
1269 }
1270 else if (1 == numBytes)
1271 {
1272 SetType(PAD1);
1273 return true;
1274 }
1275 else
1276 {
1277 return false;
1278 }
1279 }
1280 else
1281 {
1282 PLOG(PL_ERROR, "ProtoPktIPv6::Option::MakePad() error: no buffer space allocated\n");
1283 return false;
1284 }
1285 } // end ProtoPktIPv6::Option::MakePad()
1286
1287 ProtoPktIPv6::Option::Iterator::Iterator(const Extension& extension)
1288 : hdr_extension(extension), offset(2)
1289 {
1290
1291 }
1292
1293 ProtoPktIPv6::Option::Iterator::~Iterator()
1294 {
1295 }
1296
1297 bool ProtoPktIPv6::Option::Iterator::GetNextOption(Option& option)
1298 {
1299 if (offset >= hdr_extension.GetLength())
1300 {
1301
1302 return false;
1303 }
1304 else if (option.InitFromBuffer(((char*)hdr_extension.GetBuffer()) + offset,
1305 hdr_extension.GetLength() - offset))
1306 {
1307 offset += option.GetLength();
1308 return true;
1309 }
1310 else
1311 {
1312 return false;
1313 }
1314 } // end ProtoPktIPv6::Option::Iterator::GetNextOption()
1315
1316
1317 ProtoPktFRAG::ProtoPktFRAG(void* bufferPtr,
1318 unsigned int numBytes,
1319 bool initFromBuffer,
1320 bool freeOnDestruct)
1321 : ProtoPktIPv6::Extension(ProtoPktIP::FRAG, bufferPtr, numBytes, initFromBuffer, freeOnDestruct)
1322 {
1323 }
1324
1325 ProtoPktFRAG::~ProtoPktFRAG()
1326 {
1327 }
1328
1329 bool ProtoPktFRAG::InitIntoBuffer(void* bufferPtr,
1330 unsigned int numBytes,
1331 bool freeOnDestruct)
1332 {
1333 if (Extension::InitIntoBuffer(ProtoPktIP::FRAG, bufferPtr, numBytes, freeOnDestruct))
1334 {
1335 // Make sure it's big enough (8 bytes)
1336 unsigned int minLength = 8;
1337 if (GetBufferLength() < minLength)
1338 {
1339 SetLength(0);
1340 if (bufferPtr != NULL)
1341 DetachBuffer();
1342 return false;
1343 }
1344 // zero init everything (all 8 bytes)
1345 memset(AccessBuffer(), 0, 8);
1346 SetLength(minLength);
1347 return true;
1348 }
1349 else
1350 {
1351 return false;
1352 }
1353 } // end ProtoPktFRAG::InitIntoBuffer()
1354
1355
1356 ProtoPktAUTH::ProtoPktAUTH(void* bufferPtr,
1357 unsigned int numBytes,
1358 bool initFromBuffer,
1359 bool freeOnDestruct)
1360 : ProtoPktIPv6::Extension(ProtoPktIP::AUTH, bufferPtr, numBytes, initFromBuffer, freeOnDestruct)
1361 {
1362 }
1363
1364 ProtoPktAUTH::~ProtoPktAUTH()
1365 {
1366 }
1367
1368 bool ProtoPktAUTH::InitIntoBuffer(void* bufferPtr,
1369 unsigned int numBytes,
1370 bool freeOnDestruct)
1371 {
1372 if (Extension::InitIntoBuffer(ProtoPktIP::AUTH, bufferPtr, numBytes, freeOnDestruct))
1373 {
1374 // Make sure it's big enough for at least reserved, spi, and sequence fields
1375 unsigned int minLength = ((OFFSET_SEQUENCE+1) << 2);
1376 if (GetBufferLength() < minLength)
1377 {
1378 SetLength(0);
1379 if (bufferPtr != NULL)
1380 DetachBuffer();
1381 return false;
1382 }
1383 SetWord16(OFFSET_RESERVED, 0);
1384 SetLength(minLength);
1385 return true;
1386 }
1387 else
1388 {
1389 return false;
1390 }
1391 } // end ProtoPktAUTH::InitIntoBuffer()
1392
1393 bool ProtoPktAUTH::InitFromBuffer(void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
1394 {
1395 if (Extension::InitFromBuffer(ProtoPktIP::AUTH, bufferPtr, numBytes, freeOnDestruct))
1396 {
1397 if (GetBufferLength() >= ((OFFSET_SEQUENCE+1) << 2))
1398 {
1399 return true;
1400 }
1401 else
1402 {
1403 SetLength(0);
1404 if (NULL != bufferPtr)
1405 DetachBuffer();
1406 return false;
1407 }
1408 }
1409 else
1410 {
1411 return false;
1412 }
1413 } // end ProtoPktAUTH::InitFromBuffer()
1414
1415
1416 ProtoPktESP::ProtoPktESP(void* bufferPtr,
1417 unsigned int numBytes,
1418 bool freeOnDestruct)
1419 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct)
1420 {
1421 }
1422
1423 ProtoPktESP::~ProtoPktESP()
1424 {
1425 }
1426 /**
1427 * This function makes sure the buffer is big enough for at least
1428 * spi and sequence fields.
1429 */
1430 bool ProtoPktESP::InitIntoBuffer(void* bufferPtr,
1431 unsigned int numBytes,
1432 bool freeOnDestruct)
1433 {
1434 if (NULL != bufferPtr) AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1435 if (0 == GetBufferLength()) return true; // don't worry if no buffer_ptr for moment
1436 // Make sure it's big enough for at least spi and sequence fields
1437 unsigned int minLength = ((OFFSET_SEQUENCE+1) << 2);
1438 if (GetBufferLength() >= minLength)
1439 {
1440 ProtoPkt::SetLength(minLength);
1441 return true;
1442 }
1443 else
1444 {
1445 ProtoPkt::SetLength(0);
1446 if (NULL != bufferPtr)
1447 DetachBuffer();
1448 return false;
1449 }
1450 } // end ProtoPktESP::InitIntoBuffer()
1451 /**
1452 * This function makes sure the buffer is big enough for at least
1453 * spi and sequence fields.
1454 */
1455
1456 bool ProtoPktESP::InitFromBuffer(UINT16 espLength, void* bufferPtr, unsigned int numBytes, bool freeOnDestruct)
1457 {
1458 if (NULL != bufferPtr)
1459 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1460 // Make sure it's big enough for at least spi and sequence fields
1461 unsigned int minLength = ((OFFSET_SEQUENCE+1) << 2);
1462 if (GetBufferLength() >= minLength)
1463 {
1464 SetLength(espLength);
1465 return true;
1466 }
1467 else
1468 {
1469 ProtoPkt::SetLength(0);
1470 if (NULL != bufferPtr)
1471 DetachBuffer();
1472 return false;
1473 }
1474 } // end ProtoPktESP::InitFromBuffer()
1475
1476
1477 // (TBD) - move "ProtoPktDPD" implementation to the protolib/manet tree?
1478 ProtoPktDPD::ProtoPktDPD(void* bufferPtr,
1479 unsigned int numBytes,
1480 bool initFromBuffer,
1481 bool freeOnDestruct)
1482 : ProtoPktIPv6::Option(bufferPtr, numBytes, false, freeOnDestruct)
1483 {
1484 if (initFromBuffer)
1485 InitFromBuffer();
1486 else if (NULL != bufferPtr)
1487 InitIntoBuffer();
1488 }
1489
1490 ProtoPktDPD::~ProtoPktDPD()
1491 {
1492 }
1493
1494 bool ProtoPktDPD::InitFromBuffer(void* bufferPtr,
1495 unsigned int numBytes,
1496 bool freeOnDestruct)
1497 {
1498 if (NULL != bufferPtr)
1499 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1500 if ((numBytes < (OFFSET_DATA_LENGTH + 1)) ||
1501 (SMF_DPD != GetType()))
1502 {
1503 if (NULL != bufferPtr) DetachBuffer();
1504 return false;
1505 }
1506 unsigned int dataLength = GetDataLength();
1507 if ((dataLength < 1) || (numBytes < (OFFSET_DATA_LENGTH + 1 + dataLength)))
1508 {
1509 if (NULL != bufferPtr) DetachBuffer();
1510 return false;
1511 }
1512 if (GetTaggerIdLength() >= dataLength)
1513 {
1514 if (NULL != bufferPtr) DetachBuffer();
1515 return false;
1516 }
1517 return true;
1518 } // end ProtoPktDPD::InitFromBuffer()
1519
1520 bool ProtoPktDPD::GetTaggerId(ProtoAddress& addr) const
1521 {
1522 switch (GetTaggerIdType())
1523 {
1524 case TID_IPv4_ADDR:
1525 if (4 == GetTaggerIdLength())
1526 {
1527 addr.SetRawHostAddress(ProtoAddress::IPv4, GetTaggerId(), 4);
1528 return true;
1529 }
1530 else
1531 {
1532 return false;
1533 }
1534 case TID_IPv6_ADDR:
1535 if (4 == GetTaggerIdLength())
1536 {
1537 addr.SetRawHostAddress(ProtoAddress::IPv6, GetTaggerId(), 16);
1538 return true;
1539 }
1540 else
1541 {
1542 return false;
1543 }
1544 return true;
1545 default:
1546 return false;
1547 }
1548 } // end ProtoPktDPD::GetTaggerId(by address)
1549
1550 bool ProtoPktDPD::GetPktId(UINT8& value) const
1551 {
1552 if (1 == GetPktIdLength())
1553 {
1554 value = GetUINT8(OffsetPktId());
1555 return true;
1556 }
1557 else
1558 {
1559 return false;
1560 }
1561 } // end ProtoPktDPD::GetPktId(UINT8& value)
1562
1563 bool ProtoPktDPD::GetPktId(UINT16& value) const
1564 {
1565 if (2 == GetPktIdLength())
1566 {
1567 memcpy(&value, GetBuffer(OffsetPktId()), 2);
1568 value = ntohs(value);
1569 return true;
1570 }
1571 else
1572 {
1573 return false;
1574 }
1575 } // end ProtoPktDPD::GetPktId(UINT16& value)
1576
1577 bool ProtoPktDPD::GetPktId(UINT32& value) const
1578 {
1579 if (4 == GetPktIdLength())
1580 {
1581 memcpy(&value, GetBuffer(OffsetPktId()), 4);
1582 value = ntohl(value);
1583 return true;
1584 }
1585 else
1586 {
1587 return false;
1588 }
1589 } // end ProtoPktDPD::GetPktId(UINT16& value)
1590
1591 bool ProtoPktDPD::InitIntoBuffer(void* bufferPtr,
1592 unsigned int numBytes,
1593 bool freeOnDestruct)
1594 {
1595 if (NULL != bufferPtr)
1596 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1597 if (numBytes < (OFFSET_DATA_LENGTH + 1))
1598 {
1599 if (NULL != bufferPtr) DetachBuffer();
1600 return false;
1601 }
1602 // Set NULL tagger id
1603 SetUINT8(OFFSET_TID_TYPE, 0);
1604 SetDataLength(1);
1605 return 0;
1606 } // end ProtoPktDPD::InitIntoBuffer()
1607
1608 bool ProtoPktDPD::SetHAV(const char* hashAssistValue, UINT8 numBytes)
1609 {
1610 unsigned int minLength = OFFSET_HAV + numBytes;
1611 if (GetBufferLength() < minLength) return false;
1612 char* ptr = (char*)AccessBuffer(OFFSET_HAV);
1613 memcpy(ptr, hashAssistValue, numBytes);
1614 *ptr |= (char)0x80; // make sure 'H' bit is set
1615 SetDataLength(numBytes);
1616 return true;
1617 } // end ProtoPktDPD::SetHAV()
1618
1619 bool ProtoPktDPD::SetTaggerId(TaggerIdType type, const char* taggerId, UINT8 taggerIdLength)
1620 {
1621 if ((TID_NULL != type) && (0 != taggerIdLength))
1622 {
1623 unsigned int minLength = OFFSET_TID_VALUE + taggerIdLength;
1624 if (GetBufferLength() < minLength) return false;
1625 SetUINT8(OFFSET_TID_TYPE, (UINT8)(type << 4));
1626 AccessUINT8(OFFSET_TID_LENGTH) |= ((taggerIdLength - 1) & 0x0f);
1627 memcpy(AccessBuffer(OFFSET_TID_VALUE), taggerId, taggerIdLength);
1628 SetDataLength(1 + taggerIdLength);
1629 }
1630 else
1631 {
1632 SetUINT8(OFFSET_TID_TYPE, 0);
1633 SetDataLength(1);
1634 }
1635 return true;
1636 } // end ProtoPktDPD::SetTaggerId(by buffer)
1637
1638
1639 bool ProtoPktDPD::SetTaggerId(const ProtoAddress& ipAddr)
1640 {
1641 switch (ipAddr.GetType())
1642 {
1643 case ProtoAddress::IPv4:
1644 return SetTaggerId(TID_IPv4_ADDR, ipAddr.GetRawHostAddress(), 4);
1645 case ProtoAddress::IPv6:
1646 return SetTaggerId(TID_IPv6_ADDR, ipAddr.GetRawHostAddress(), 16);
1647 default:
1648 PLOG(PL_ERROR, "ProtoPktDPD::SetTaggerId() error: invalid address type\n");
1649 return false;
1650 }
1651 } // ProtoPktDPD::SetTaggerId(by address)
1652
1653
1654
1655 // begin ProtoPktMobile implementation
1656 ProtoPktMobile::ProtoPktMobile(void* bufferPtr,
1657 unsigned int numBytes,
1658 bool initFromBuffer,
1659 bool freeOnDestruct)
1660 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct)
1661 {
1662 if (NULL != bufferPtr)
1663 {
1664 if (initFromBuffer)
1665 InitFromBuffer();
1666 else
1667 InitIntoBuffer();
1668 }
1669 }
1670
1671 ProtoPktMobile::~ProtoPktMobile()
1672 {
1673 }
1674
1675 bool ProtoPktMobile::InitIntoBuffer(void* bufferPtr, unsigned int bufferBytes, bool freeOnDestruct)
1676 {
1677 if (NULL != bufferPtr)
1678 {
1679 if (bufferBytes < 8)
1680 return false;
1681 else
1682 AttachBuffer(bufferPtr, bufferBytes, freeOnDestruct);
1683 }
1684 else if (GetBufferLength() < 8)
1685 {
1686 return false;
1687 }
1688 SetProtocol(ProtoPktIP::RESERVED);
1689 SetUINT8(OFFSET_RESERVED, 0);
1690 SetChecksum(0);
1691 SetLength(8);
1692 return true;
1693 } // end ProtoPktMobile::InitIntoBuffer()
1694
1695
1696 void ProtoPktMobile::SetDstAddr(const ProtoAddress& addr, bool calculateChecksum)
1697 {
1698 memcpy((char*)AccessBuffer32(OFFSET_DST_ADDR), addr.GetRawHostAddress(), 4); // (TBD) leverage alignment?
1699 if (calculateChecksum) CalculateChecksum(); // (TBD) is it worth it to incrementally update
1700 } // end ProtoPktMobile::SetDstAddr()
1701
1702 bool ProtoPktMobile::SetSrcAddr(const ProtoAddress& addr, bool calculateChecksum)
1703 {
1704 if (GetBufferLength() < 12) return false;
1705 memcpy((char*)AccessBuffer32(OFFSET_SRC_ADDR), addr.GetRawHostAddress(), 4); // (TBD) leverage alignment?
1706 if (calculateChecksum) CalculateChecksum(); // (TBD) is it worth it to incrementally update
1707 SetFlag(FLAG_SRC);
1708 SetLength(12);
1709 return true;
1710 } // end ProtoPktMobile::SetDstAddr()
1711
1712 // Return header checksum in host byte order
1713 UINT16 ProtoPktMobile::CalculateChecksum(bool set)
1714 {
1715 UINT32 sum = 0;
1716 UINT16 savedSum = GetChecksum();
1717 SetChecksum(0);
1718 // Calculate checksum, skipping checksum field
1719 unsigned int headerLen = FlagIsSet(FLAG_SRC) ? 12/2 : 8/2;
1720 for (unsigned int i = 0; i < headerLen; i++)
1721 sum += GetWord16(i);
1722 while (sum >> 16)
1723 sum = (sum & 0x0000ffff) + (sum >> 16);
1724 sum = ~sum;
1725 if (set)
1726 SetChecksum(sum);
1727 else
1728 SetChecksum(savedSum);
1729 return sum;
1730 } // ProtoPktMobile::CalculateChecksum()
1731
1732 bool ProtoPktMobile::InitFromBuffer(void* bufferPtr,
1733 unsigned int numBytes,
1734 bool freeOnDestruct)
1735 {
1736 if (NULL != bufferPtr)
1737 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1738 UINT16 minBytes = 8;
1739 if (GetBufferLength() > OFFSET_FLAGS)
1740 {
1741 if (FlagIsSet(FLAG_SRC))
1742 minBytes = 12;
1743 }
1744 if (GetBufferLength() < minBytes)
1745 {
1746 ProtoPkt::SetLength(0);
1747 if (NULL != bufferPtr) DetachBuffer();
1748 return false;
1749 }
1750 ProtoPkt::SetLength(numBytes);
1751 return true;
1752 } // end ProtoPktMobile::InitFromBuffer()
1753
1754 bool ProtoPktMobile::GetSrcAddr(ProtoAddress& src) const
1755 {
1756 if (FlagIsSet(FLAG_SRC))
1757 {
1758 src.SetRawHostAddress(ProtoAddress::IPv4, (char*)GetBuffer32(OFFSET_DST_ADDR), 4);
1759 return true;
1760 }
1761 else
1762 {
1763 src.Invalidate();
1764 return false;
1765 }
1766 } // end ProtoPktMobile::GetSrcAddr()
1767
1768
1769 bool ProtoPktDPD::SetPktId(const char* pktId, UINT8 pktIdLength)
1770 {
1771 unsigned int taggerIdLength = GetTaggerIdLength();
1772 unsigned int offsetPktId = OFFSET_TID_VALUE + taggerIdLength;
1773 unsigned int minLength = offsetPktId + pktIdLength;
1774 if (GetBufferLength() < minLength) return false;
1775 memcpy(AccessBuffer(offsetPktId), pktId, pktIdLength);
1776 SetDataLength(1 + taggerIdLength + pktIdLength);
1777 return true;
1778 }
1779
1780 ProtoPktUDP::ProtoPktUDP(void* bufferPtr,
1781 unsigned int numBytes,
1782 bool initFromBuffer,
1783 bool freeOnDestruct)
1784 : ProtoPkt(bufferPtr, numBytes, freeOnDestruct)
1785 {
1786 if (NULL != bufferPtr)
1787 {
1788 if (initFromBuffer)
1789 InitFromBuffer();
1790 else
1791 InitIntoBuffer();
1792 }
1793 }
1794
1795 ProtoPktUDP::~ProtoPktUDP()
1796 {
1797 }
1798
1799 bool ProtoPktUDP::InitFromPacket(ProtoPktIP& ipPkt)
1800 {
1801 switch (ipPkt.GetVersion())
1802 {
1803 case 4:
1804 {
1805 // (TBD) support IPv4 extended headers
1806 ProtoPktIPv4 ip4Pkt(ipPkt);
1807 if (ProtoPktIP::UDP == ip4Pkt.GetProtocol())
1808 {
1809 return InitFromBuffer(ip4Pkt.AccessPayload(), ip4Pkt.GetPayloadLength(), false);
1810 }
1811 else
1812 {
1813 return false; // not a UDP packet
1814 }
1815 break;
1816 }
1817 case 6:
1818 {
1819 ProtoPktIPv6 ip6Pkt(ipPkt);
1820 if (ip6Pkt.HasExtendedHeader())
1821 {
1822 unsigned int extHeaderLength = 0;
1823 ProtoPktIPv6::Extension::Iterator extIterator(ip6Pkt);
1824 ProtoPktIPv6::Extension ext;
1825 while (extIterator.GetNextExtension(ext))
1826 {
1827 extHeaderLength += ext.GetLength();
1828 if (ProtoPktIP::UDP == ext.GetNextHeader())
1829 {
1830 void* udpBuffer = (char*)ip6Pkt.AccessPayload() + extHeaderLength;
1831 unsigned int udpLength = ip6Pkt.GetPayloadLength() - extHeaderLength;
1832 return InitFromBuffer(udpBuffer, udpLength, false);
1833 }
1834 }
1835 return false; // not a UDP packet
1836 }
1837 else if (ProtoPktIP::UDP == ip6Pkt.GetNextHeader())
1838 {
1839 return InitFromBuffer(ip6Pkt.AccessPayload(), ip6Pkt.GetPayloadLength(), false);
1840 }
1841 else
1842 {
1843 return false; // not a UDP packet
1844 }
1845 break;
1846 }
1847 default:
1848 PLOG(PL_ERROR, "ProtoPktUDP::InitFromPacket() error: bad IP packet version: %d\n", ipPkt.GetVersion());
1849 return false;
1850 }
1851 return true;
1852 } // end ProtoPktUDP::InitFromPacket()
1853
1854 bool ProtoPktUDP::InitFromBuffer(void* bufferPtr,
1855 unsigned int numBytes,
1856 bool freeOnDestruct)
1857 {
1858 if (NULL != bufferPtr)
1859 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1860 UINT16 totalLen = GetPayloadLength() + 8;
1861 if (totalLen > GetBufferLength())
1862 {
1863 ProtoPkt::SetLength(0);
1864 if (NULL != bufferPtr) DetachBuffer();
1865 return false;
1866 }
1867 else
1868 {
1869 // (TBD) We could validate the checksum, too?
1870 ProtoPkt::SetLength(totalLen);
1871 return true;
1872 }
1873 } // end bool ProtoPktUDP::InitFromBuffer()
1874
1875 bool ProtoPktUDP::InitIntoBuffer(void* bufferPtr,
1876 unsigned int numBytes,
1877 bool freeOnDestruct)
1878 {
1879 if (NULL != bufferPtr)
1880 {
1881 if (numBytes < 8) // UDP header size
1882 return false;
1883 else
1884 AttachBuffer(bufferPtr, numBytes, freeOnDestruct);
1885 }
1886 if (GetBufferLength() < 8) return false;
1887 SetChecksum(0);
1888 return true;
1889 } // end ProtoPktUDP::InitIntoBuffer()
1890
1891 UINT16 ProtoPktUDP::ComputeChecksum(ProtoPktIP& ipPkt) const
1892 {
1893 UINT32 sum = 0;
1894 // 1) Calculate pseudo-header part
1895 switch(ipPkt.GetVersion())
1896 {
1897 case 4:
1898 {
1899 ProtoPktIPv4 ipv4Pkt(ipPkt);
1900 // a) src/dst addr pseudo header portion
1901 const UINT16* ptr = (const UINT16*)ipv4Pkt.GetSrcAddrPtr();
1902 int addrEndex = ProtoPktIPv4::ADDR_LEN; // note src + dst = 2 addresses
1903 for (int i = 0; i < addrEndex; i++)
1904 sum += (UINT16)ntohs(ptr[i]);
1905 // b) protocol & "total length" pseudo header portions
1906 sum += (UINT16)ipv4Pkt.GetProtocol();
1907 sum += (UINT16)GetLength(); // UDP length
1908 break;
1909 }
1910 case 6:
1911 {
1912 ProtoPktIPv6 ipv6Pkt(ipPkt);
1913 // a) src/dst addr pseudo header portion
1914 const UINT16* ptr = (const UINT16*)ipv6Pkt.GetSrcAddrPtr();
1915 int addrEndex = ProtoPktIPv6::ADDR_LEN; // note src + dst = 2 addresses
1916 for (int i = 0; i < addrEndex; i++)
1917 sum += (UINT16)ntohs(ptr[i]);
1918 sum += (UINT16)GetLength(); // UDP length
1919 sum += (UINT16)ipv6Pkt.GetNextHeader();
1920 break;
1921 }
1922 default:
1923 return 0;
1924 }
1925 // 2) UDP header part, sans "checksum" field
1926 unsigned int i;
1927 for (i = 0; i < OFFSET_CHECKSUM; i++)
1928 sum += GetWord16(i);
1929 // 3) UDP payload part (note adjustment for odd number of payload bytes)
1930 unsigned int dataEndex = GetLength();
1931 if (0 != (dataEndex & 0x01))
1932 sum += (UINT16)((UINT16)GetUINT8(dataEndex-1) << 8);
1933 dataEndex >>= 1; // convert from bytes to UINT16 index
1934 for (i = (OFFSET_CHECKSUM+1); i < dataEndex; i++)
1935 sum += GetWord16(i);
1936 // 4) Carry as needed
1937 while (0 != (sum >> 16))
1938 sum = (sum & 0x0000ffff) + (sum >> 16);
1939 sum = ~sum;
1940
1941 // 5) ZERO check/correct as needed
1942 if (0 == sum) sum = 0x0000ffff;
1943 return (UINT16)sum;
1944 } // end ProtoPktUDP::CalculateChecksum()
1945
1946