1 /*
2 * Copyright (c) 2016, The OpenThread Authors.
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * 1. Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * 2. Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * 3. Neither the name of the copyright holder nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
20 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 * POSSIBILITY OF SUCH DAMAGE.
27 */
28
29 /**
30 * @file
31 * This file implements 6LoWPAN header compression.
32 */
33
34 #include "lowpan.hpp"
35
36 #include "common/code_utils.hpp"
37 #include "common/debug.hpp"
38 #include "common/encoding.hpp"
39 #include "common/instance.hpp"
40 #include "common/locator_getters.hpp"
41 #include "net/ip6.hpp"
42 #include "net/udp6.hpp"
43 #include "thread/network_data_leader.hpp"
44 #include "thread/thread_netif.hpp"
45
46 using ot::Encoding::BigEndian::HostSwap16;
47 using ot::Encoding::BigEndian::ReadUint16;
48 using ot::Encoding::BigEndian::WriteUint16;
49
50 namespace ot {
51 namespace Lowpan {
52
Lowpan(Instance & aInstance)53 Lowpan::Lowpan(Instance &aInstance)
54 : InstanceLocator(aInstance)
55 {
56 }
57
CopyContext(const Context & aContext,Ip6::Address & aAddress)58 void Lowpan::CopyContext(const Context &aContext, Ip6::Address &aAddress)
59 {
60 aAddress.SetPrefix(aContext.mPrefix);
61 }
62
ComputeIid(const Mac::Address & aMacAddr,const Context & aContext,Ip6::Address & aIpAddress)63 Error Lowpan::ComputeIid(const Mac::Address &aMacAddr, const Context &aContext, Ip6::Address &aIpAddress)
64 {
65 Error error = kErrorNone;
66
67 switch (aMacAddr.GetType())
68 {
69 case Mac::Address::kTypeShort:
70 aIpAddress.GetIid().SetToLocator(aMacAddr.GetShort());
71 break;
72
73 case Mac::Address::kTypeExtended:
74 aIpAddress.GetIid().SetFromExtAddress(aMacAddr.GetExtended());
75 break;
76
77 default:
78 ExitNow(error = kErrorParse);
79 }
80
81 if (aContext.mPrefix.GetLength() > 64)
82 {
83 for (int i = (aContext.mPrefix.GetLength() & ~7); i < aContext.mPrefix.GetLength(); i++)
84 {
85 aIpAddress.mFields.m8[i / CHAR_BIT] &= ~(0x80 >> (i % CHAR_BIT));
86 aIpAddress.mFields.m8[i / CHAR_BIT] |= aContext.mPrefix.GetBytes()[i / CHAR_BIT] & (0x80 >> (i % CHAR_BIT));
87 }
88 }
89
90 exit:
91 return error;
92 }
93
CompressSourceIid(const Mac::Address & aMacAddr,const Ip6::Address & aIpAddr,const Context & aContext,uint16_t & aHcCtl,BufferWriter & aBuf)94 Error Lowpan::CompressSourceIid(const Mac::Address &aMacAddr,
95 const Ip6::Address &aIpAddr,
96 const Context & aContext,
97 uint16_t & aHcCtl,
98 BufferWriter & aBuf)
99 {
100 Error error = kErrorNone;
101 BufferWriter buf = aBuf;
102 Ip6::Address ipaddr;
103 Mac::Address tmp;
104
105 IgnoreError(ComputeIid(aMacAddr, aContext, ipaddr));
106
107 if (ipaddr.GetIid() == aIpAddr.GetIid())
108 {
109 aHcCtl |= kHcSrcAddrMode3;
110 }
111 else
112 {
113 tmp.SetShort(aIpAddr.GetIid().GetLocator());
114 IgnoreError(ComputeIid(tmp, aContext, ipaddr));
115
116 if (ipaddr.GetIid() == aIpAddr.GetIid())
117 {
118 aHcCtl |= kHcSrcAddrMode2;
119 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8 + 14, 2));
120 }
121 else
122 {
123 aHcCtl |= kHcSrcAddrMode1;
124 SuccessOrExit(error = buf.Write(aIpAddr.GetIid().GetBytes(), Ip6::InterfaceIdentifier::kSize));
125 }
126 }
127
128 exit:
129 if (error == kErrorNone)
130 {
131 aBuf = buf;
132 }
133
134 return error;
135 }
136
CompressDestinationIid(const Mac::Address & aMacAddr,const Ip6::Address & aIpAddr,const Context & aContext,uint16_t & aHcCtl,BufferWriter & aBuf)137 Error Lowpan::CompressDestinationIid(const Mac::Address &aMacAddr,
138 const Ip6::Address &aIpAddr,
139 const Context & aContext,
140 uint16_t & aHcCtl,
141 BufferWriter & aBuf)
142 {
143 Error error = kErrorNone;
144 BufferWriter buf = aBuf;
145 Ip6::Address ipaddr;
146 Mac::Address tmp;
147
148 IgnoreError(ComputeIid(aMacAddr, aContext, ipaddr));
149
150 if (ipaddr.GetIid() == aIpAddr.GetIid())
151 {
152 aHcCtl |= kHcDstAddrMode3;
153 }
154 else
155 {
156 tmp.SetShort(aIpAddr.GetIid().GetLocator());
157 IgnoreError(ComputeIid(tmp, aContext, ipaddr));
158
159 if (ipaddr.GetIid() == aIpAddr.GetIid())
160 {
161 aHcCtl |= kHcDstAddrMode2;
162 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8 + 14, 2));
163 }
164 else
165 {
166 aHcCtl |= kHcDstAddrMode1;
167 SuccessOrExit(error = buf.Write(aIpAddr.GetIid().GetBytes(), Ip6::InterfaceIdentifier::kSize));
168 }
169 }
170
171 exit:
172 if (error == kErrorNone)
173 {
174 aBuf = buf;
175 }
176
177 return error;
178 }
179
CompressMulticast(const Ip6::Address & aIpAddr,uint16_t & aHcCtl,BufferWriter & aBuf)180 Error Lowpan::CompressMulticast(const Ip6::Address &aIpAddr, uint16_t &aHcCtl, BufferWriter &aBuf)
181 {
182 Error error = kErrorNone;
183 BufferWriter buf = aBuf;
184 Context multicastContext;
185
186 aHcCtl |= kHcMulticast;
187
188 for (unsigned int i = 2; i < sizeof(Ip6::Address); i++)
189 {
190 if (aIpAddr.mFields.m8[i])
191 {
192 // Check if multicast address can be compressed to 8-bits (ff02::00xx)
193 if (aIpAddr.mFields.m8[1] == 0x02 && i >= 15)
194 {
195 aHcCtl |= kHcDstAddrMode3;
196 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8[15]));
197 }
198 // Check if multicast address can be compressed to 32-bits (ffxx::00xx:xxxx)
199 else if (i >= 13)
200 {
201 aHcCtl |= kHcDstAddrMode2;
202 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8[1]));
203 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8 + 13, 3));
204 }
205 // Check if multicast address can be compressed to 48-bits (ffxx::00xx:xxxx:xxxx)
206 else if (i >= 11)
207 {
208 aHcCtl |= kHcDstAddrMode1;
209 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8[1]));
210 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8 + 11, 5));
211 }
212 else
213 {
214 // Check if multicast address can be compressed using Context ID 0.
215 if (Get<NetworkData::Leader>().GetContext(0, multicastContext) == kErrorNone &&
216 multicastContext.mPrefix.GetLength() == aIpAddr.mFields.m8[3] &&
217 memcmp(multicastContext.mPrefix.GetBytes(), aIpAddr.mFields.m8 + 4, 8) == 0)
218 {
219 aHcCtl |= kHcDstAddrContext | kHcDstAddrMode0;
220 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8 + 1, 2));
221 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8 + 12, 4));
222 }
223 else
224 {
225 SuccessOrExit(error = buf.Write(aIpAddr.mFields.m8, sizeof(Ip6::Address)));
226 }
227 }
228
229 break;
230 }
231 }
232
233 exit:
234 if (error == kErrorNone)
235 {
236 aBuf = buf;
237 }
238
239 return error;
240 }
241
Compress(Message & aMessage,const Mac::Address & aMacSource,const Mac::Address & aMacDest,BufferWriter & aBuf)242 Error Lowpan::Compress(Message & aMessage,
243 const Mac::Address &aMacSource,
244 const Mac::Address &aMacDest,
245 BufferWriter & aBuf)
246 {
247 Error error;
248 uint8_t headerDepth = 0xff;
249
250 do
251 {
252 error = Compress(aMessage, aMacSource, aMacDest, aBuf, headerDepth);
253 } while ((error != kErrorNone) && (headerDepth > 0));
254
255 return error;
256 }
257
Compress(Message & aMessage,const Mac::Address & aMacSource,const Mac::Address & aMacDest,BufferWriter & aBuf,uint8_t & aHeaderDepth)258 Error Lowpan::Compress(Message & aMessage,
259 const Mac::Address &aMacSource,
260 const Mac::Address &aMacDest,
261 BufferWriter & aBuf,
262 uint8_t & aHeaderDepth)
263 {
264 Error error = kErrorNone;
265 NetworkData::Leader &networkData = Get<NetworkData::Leader>();
266 uint16_t startOffset = aMessage.GetOffset();
267 BufferWriter buf = aBuf;
268 uint16_t hcCtl = kHcDispatch;
269 Ip6::Header ip6Header;
270 uint8_t * ip6HeaderBytes = reinterpret_cast<uint8_t *>(&ip6Header);
271 Context srcContext, dstContext;
272 bool srcContextValid, dstContextValid;
273 uint8_t nextHeader;
274 uint8_t ecn;
275 uint8_t dscp;
276 uint8_t headerDepth = 0;
277 uint8_t headerMaxDepth = aHeaderDepth;
278
279 SuccessOrExit(error = aMessage.Read(aMessage.GetOffset(), ip6Header));
280
281 srcContextValid =
282 (networkData.GetContext(ip6Header.GetSource(), srcContext) == kErrorNone && srcContext.mCompressFlag);
283
284 if (!srcContextValid)
285 {
286 IgnoreError(networkData.GetContext(0, srcContext));
287 }
288
289 dstContextValid =
290 (networkData.GetContext(ip6Header.GetDestination(), dstContext) == kErrorNone && dstContext.mCompressFlag);
291
292 if (!dstContextValid)
293 {
294 IgnoreError(networkData.GetContext(0, dstContext));
295 }
296
297 // Lowpan HC Control Bits
298 SuccessOrExit(error = buf.Advance(sizeof(hcCtl)));
299
300 // Context Identifier
301 if (srcContext.mContextId != 0 || dstContext.mContextId != 0)
302 {
303 hcCtl |= kHcContextId;
304 SuccessOrExit(error = buf.Write(((srcContext.mContextId << 4) | dstContext.mContextId) & 0xff));
305 }
306
307 dscp = ((ip6HeaderBytes[0] << 2) & 0x3c) | (ip6HeaderBytes[1] >> 6);
308 ecn = (ip6HeaderBytes[1] << 2) & 0xc0;
309
310 // Flow Label
311 if (((ip6HeaderBytes[1] & 0x0f) == 0) && ((ip6HeaderBytes[2]) == 0) && ((ip6HeaderBytes[3]) == 0))
312 {
313 if (dscp == 0 && ecn == 0)
314 {
315 // Elide Flow Label and Traffic Class.
316 hcCtl |= kHcTrafficClass | kHcFlowLabel;
317 }
318 else
319 {
320 // Elide Flow Label and carry Traffic Class in-line.
321 hcCtl |= kHcFlowLabel;
322
323 SuccessOrExit(error = buf.Write(ecn | dscp));
324 }
325 }
326 else if (dscp == 0)
327 {
328 // Carry Flow Label and ECN only with 2-bit padding.
329 hcCtl |= kHcTrafficClass;
330
331 SuccessOrExit(error = buf.Write(ecn | (ip6HeaderBytes[1] & 0x0f)));
332 SuccessOrExit(error = buf.Write(ip6HeaderBytes + 2, 2));
333 }
334 else
335 {
336 // Carry Flow Label and Traffic Class in-line.
337 SuccessOrExit(error = buf.Write(ecn | dscp));
338 SuccessOrExit(error = buf.Write(ip6HeaderBytes[1] & 0x0f));
339 SuccessOrExit(error = buf.Write(ip6HeaderBytes + 2, 2));
340 }
341
342 // Next Header
343 switch (ip6Header.GetNextHeader())
344 {
345 case Ip6::kProtoHopOpts:
346 case Ip6::kProtoUdp:
347 case Ip6::kProtoIp6:
348 if (headerDepth + 1 < headerMaxDepth)
349 {
350 hcCtl |= kHcNextHeader;
351 break;
352 }
353 OT_FALL_THROUGH;
354
355 default:
356 SuccessOrExit(error = buf.Write(static_cast<uint8_t>(ip6Header.GetNextHeader())));
357 break;
358 }
359
360 // Hop Limit
361 switch (ip6Header.GetHopLimit())
362 {
363 case 1:
364 hcCtl |= kHcHopLimit1;
365 break;
366
367 case 64:
368 hcCtl |= kHcHopLimit64;
369 break;
370
371 case 255:
372 hcCtl |= kHcHopLimit255;
373 break;
374
375 default:
376 SuccessOrExit(error = buf.Write(ip6Header.GetHopLimit()));
377 break;
378 }
379
380 // Source Address
381 if (ip6Header.GetSource().IsUnspecified())
382 {
383 hcCtl |= kHcSrcAddrContext;
384 }
385 else if (ip6Header.GetSource().IsLinkLocal())
386 {
387 SuccessOrExit(error = CompressSourceIid(aMacSource, ip6Header.GetSource(), srcContext, hcCtl, buf));
388 }
389 else if (srcContextValid)
390 {
391 hcCtl |= kHcSrcAddrContext;
392 SuccessOrExit(error = CompressSourceIid(aMacSource, ip6Header.GetSource(), srcContext, hcCtl, buf));
393 }
394 else
395 {
396 SuccessOrExit(error = buf.Write(ip6Header.GetSource().mFields.m8, sizeof(ip6Header.GetSource())));
397 }
398
399 // Destination Address
400 if (ip6Header.GetDestination().IsMulticast())
401 {
402 SuccessOrExit(error = CompressMulticast(ip6Header.GetDestination(), hcCtl, buf));
403 }
404 else if (ip6Header.GetDestination().IsLinkLocal())
405 {
406 SuccessOrExit(error = CompressDestinationIid(aMacDest, ip6Header.GetDestination(), dstContext, hcCtl, buf));
407 }
408 else if (dstContextValid)
409 {
410 hcCtl |= kHcDstAddrContext;
411 SuccessOrExit(error = CompressDestinationIid(aMacDest, ip6Header.GetDestination(), dstContext, hcCtl, buf));
412 }
413 else
414 {
415 SuccessOrExit(error = buf.Write(&ip6Header.GetDestination(), sizeof(ip6Header.GetDestination())));
416 }
417
418 headerDepth++;
419
420 aMessage.MoveOffset(sizeof(ip6Header));
421
422 nextHeader = static_cast<uint8_t>(ip6Header.GetNextHeader());
423
424 while (headerDepth < headerMaxDepth)
425 {
426 switch (nextHeader)
427 {
428 case Ip6::kProtoHopOpts:
429 SuccessOrExit(error = CompressExtensionHeader(aMessage, buf, nextHeader));
430 break;
431
432 case Ip6::kProtoUdp:
433 error = CompressUdp(aMessage, buf);
434 ExitNow();
435
436 case Ip6::kProtoIp6:
437 // For IP-in-IP the NH bit of the LOWPAN_NHC encoding MUST be set to zero.
438 SuccessOrExit(error = buf.Write(kExtHdrDispatch | kExtHdrEidIp6));
439
440 error = Compress(aMessage, aMacSource, aMacDest, buf);
441
442 OT_FALL_THROUGH;
443
444 default:
445 ExitNow();
446 }
447
448 headerDepth++;
449 }
450
451 exit:
452 aHeaderDepth = headerDepth;
453
454 if (error == kErrorNone)
455 {
456 IgnoreError(aBuf.Write(hcCtl >> 8));
457 IgnoreError(aBuf.Write(hcCtl & 0xff));
458 aBuf = buf;
459 }
460 else
461 {
462 aMessage.SetOffset(startOffset);
463 }
464
465 return error;
466 }
467
CompressExtensionHeader(Message & aMessage,BufferWriter & aBuf,uint8_t & aNextHeader)468 Error Lowpan::CompressExtensionHeader(Message &aMessage, BufferWriter &aBuf, uint8_t &aNextHeader)
469 {
470 Error error = kErrorNone;
471 BufferWriter buf = aBuf;
472 uint16_t startOffset = aMessage.GetOffset();
473 Ip6::ExtensionHeader extHeader;
474 uint16_t len;
475 uint8_t padLength = 0;
476 uint8_t tmpByte;
477
478 SuccessOrExit(error = aMessage.Read(aMessage.GetOffset(), extHeader));
479 aMessage.MoveOffset(sizeof(extHeader));
480
481 tmpByte = kExtHdrDispatch | kExtHdrEidHbh;
482
483 switch (extHeader.GetNextHeader())
484 {
485 case Ip6::kProtoUdp:
486 case Ip6::kProtoIp6:
487 tmpByte |= kExtHdrNextHeader;
488 break;
489
490 default:
491 SuccessOrExit(error = buf.Write(tmpByte));
492 tmpByte = static_cast<uint8_t>(extHeader.GetNextHeader());
493 break;
494 }
495
496 SuccessOrExit(error = buf.Write(tmpByte));
497
498 len = (extHeader.GetLength() + 1) * 8 - sizeof(extHeader);
499
500 // RFC 6282 does not support compressing large extension headers
501 VerifyOrExit(len <= kExtHdrMaxLength, error = kErrorFailed);
502
503 // RFC 6282 says: "IPv6 Hop-by-Hop and Destination Options Headers may use a trailing
504 // Pad1 or PadN to achieve 8-octet alignment. When there is a single trailing Pad1 or PadN
505 // option of 7 octets or less and the containing header is a multiple of 8 octets, the trailing
506 // Pad1 or PadN option MAY be elided by the compressor."
507 if (aNextHeader == Ip6::kProtoHopOpts || aNextHeader == Ip6::kProtoDstOpts)
508 {
509 uint16_t offset = aMessage.GetOffset();
510 Ip6::OptionHeader optionHeader;
511
512 while ((offset - aMessage.GetOffset()) < len)
513 {
514 SuccessOrExit(error = aMessage.Read(offset, optionHeader));
515
516 if (optionHeader.GetType() == Ip6::OptionPad1::kType)
517 {
518 offset += sizeof(Ip6::OptionPad1);
519 }
520 else
521 {
522 offset += sizeof(optionHeader) + optionHeader.GetLength();
523 }
524 }
525
526 // Check if the last option can be compressed.
527 if (optionHeader.GetType() == Ip6::OptionPad1::kType)
528 {
529 padLength = sizeof(Ip6::OptionPad1);
530 }
531 else if (optionHeader.GetType() == Ip6::OptionPadN::kType)
532 {
533 padLength = sizeof(optionHeader) + optionHeader.GetLength();
534 }
535
536 len -= padLength;
537 }
538
539 VerifyOrExit(aMessage.GetOffset() + len + padLength <= aMessage.GetLength(), error = kErrorParse);
540
541 aNextHeader = static_cast<uint8_t>(extHeader.GetNextHeader());
542
543 SuccessOrExit(error = buf.Write(static_cast<uint8_t>(len)));
544 SuccessOrExit(error = buf.Write(aMessage, static_cast<uint8_t>(len)));
545 aMessage.MoveOffset(len + padLength);
546
547 exit:
548 if (error == kErrorNone)
549 {
550 aBuf = buf;
551 }
552 else
553 {
554 aMessage.SetOffset(startOffset);
555 }
556
557 return error;
558 }
559
CompressUdp(Message & aMessage,BufferWriter & aBuf)560 Error Lowpan::CompressUdp(Message &aMessage, BufferWriter &aBuf)
561 {
562 Error error = kErrorNone;
563 BufferWriter buf = aBuf;
564 uint16_t startOffset = aMessage.GetOffset();
565 Ip6::Udp::Header udpHeader;
566 uint16_t source;
567 uint16_t destination;
568
569 SuccessOrExit(error = aMessage.Read(aMessage.GetOffset(), udpHeader));
570
571 source = udpHeader.GetSourcePort();
572 destination = udpHeader.GetDestinationPort();
573
574 if ((source & 0xfff0) == 0xf0b0 && (destination & 0xfff0) == 0xf0b0)
575 {
576 SuccessOrExit(error = buf.Write(kUdpDispatch | 3));
577 SuccessOrExit(error = buf.Write((((source & 0xf) << 4) | (destination & 0xf)) & 0xff));
578 }
579 else if ((source & 0xff00) == 0xf000)
580 {
581 SuccessOrExit(error = buf.Write(kUdpDispatch | 2));
582 SuccessOrExit(error = buf.Write(source & 0xff));
583 SuccessOrExit(error = buf.Write(destination >> 8));
584 SuccessOrExit(error = buf.Write(destination & 0xff));
585 }
586 else if ((destination & 0xff00) == 0xf000)
587 {
588 SuccessOrExit(error = buf.Write(kUdpDispatch | 1));
589 SuccessOrExit(error = buf.Write(source >> 8));
590 SuccessOrExit(error = buf.Write(source & 0xff));
591 SuccessOrExit(error = buf.Write(destination & 0xff));
592 }
593 else
594 {
595 SuccessOrExit(error = buf.Write(kUdpDispatch));
596 SuccessOrExit(error = buf.Write(&udpHeader, Ip6::Udp::Header::kLengthFieldOffset));
597 }
598
599 SuccessOrExit(error =
600 buf.Write(reinterpret_cast<uint8_t *>(&udpHeader) + Ip6::Udp::Header::kChecksumFieldOffset, 2));
601
602 aMessage.MoveOffset(sizeof(udpHeader));
603
604 exit:
605 if (error == kErrorNone)
606 {
607 aBuf = buf;
608 }
609 else
610 {
611 aMessage.SetOffset(startOffset);
612 }
613
614 return error;
615 }
616
DispatchToNextHeader(uint8_t aDispatch,uint8_t & aNextHeader)617 Error Lowpan::DispatchToNextHeader(uint8_t aDispatch, uint8_t &aNextHeader)
618 {
619 Error error = kErrorNone;
620
621 if ((aDispatch & kExtHdrDispatchMask) == kExtHdrDispatch)
622 {
623 switch (aDispatch & kExtHdrEidMask)
624 {
625 case kExtHdrEidHbh:
626 aNextHeader = Ip6::kProtoHopOpts;
627 ExitNow();
628
629 case kExtHdrEidRouting:
630 aNextHeader = Ip6::kProtoRouting;
631 ExitNow();
632
633 case kExtHdrEidFragment:
634 aNextHeader = Ip6::kProtoFragment;
635 ExitNow();
636
637 case kExtHdrEidDst:
638 aNextHeader = Ip6::kProtoDstOpts;
639 ExitNow();
640
641 case kExtHdrEidIp6:
642 aNextHeader = Ip6::kProtoIp6;
643 ExitNow();
644 }
645 }
646 else if ((aDispatch & kUdpDispatchMask) == kUdpDispatch)
647 {
648 aNextHeader = Ip6::kProtoUdp;
649 ExitNow();
650 }
651
652 error = kErrorParse;
653
654 exit:
655 return error;
656 }
657
DecompressBaseHeader(Ip6::Header & aIp6Header,bool & aCompressedNextHeader,const Mac::Address & aMacSource,const Mac::Address & aMacDest,const uint8_t * aBuf,uint16_t aBufLength)658 int Lowpan::DecompressBaseHeader(Ip6::Header & aIp6Header,
659 bool & aCompressedNextHeader,
660 const Mac::Address &aMacSource,
661 const Mac::Address &aMacDest,
662 const uint8_t * aBuf,
663 uint16_t aBufLength)
664 {
665 NetworkData::Leader &networkData = Get<NetworkData::Leader>();
666 Error error = kErrorParse;
667 const uint8_t * cur = aBuf;
668 const uint8_t * end = aBuf + aBufLength;
669 uint16_t hcCtl;
670 Context srcContext, dstContext;
671 bool srcContextValid = true, dstContextValid = true;
672 uint8_t nextHeader;
673 uint8_t * bytes;
674
675 VerifyOrExit(cur + 2 <= end);
676 hcCtl = ReadUint16(cur);
677 cur += 2;
678
679 // check Dispatch bits
680 VerifyOrExit((hcCtl & kHcDispatchMask) == kHcDispatch);
681
682 // Context Identifier
683 srcContext.mPrefix.SetLength(0);
684 dstContext.mPrefix.SetLength(0);
685
686 if ((hcCtl & kHcContextId) != 0)
687 {
688 VerifyOrExit(cur < end);
689
690 if (networkData.GetContext(cur[0] >> 4, srcContext) != kErrorNone)
691 {
692 srcContextValid = false;
693 }
694
695 if (networkData.GetContext(cur[0] & 0xf, dstContext) != kErrorNone)
696 {
697 dstContextValid = false;
698 }
699
700 cur++;
701 }
702 else
703 {
704 IgnoreError(networkData.GetContext(0, srcContext));
705 IgnoreError(networkData.GetContext(0, dstContext));
706 }
707
708 memset(&aIp6Header, 0, sizeof(aIp6Header));
709 aIp6Header.Init();
710
711 // Traffic Class and Flow Label
712 if ((hcCtl & kHcTrafficFlowMask) != kHcTrafficFlow)
713 {
714 VerifyOrExit(cur < end);
715
716 bytes = reinterpret_cast<uint8_t *>(&aIp6Header);
717 bytes[1] |= (cur[0] & 0xc0) >> 2;
718
719 if ((hcCtl & kHcTrafficClass) == 0)
720 {
721 bytes[0] |= (cur[0] >> 2) & 0x0f;
722 bytes[1] |= (cur[0] << 6) & 0xc0;
723 cur++;
724 }
725
726 if ((hcCtl & kHcFlowLabel) == 0)
727 {
728 VerifyOrExit(cur + 3 <= end);
729 bytes[1] |= cur[0] & 0x0f;
730 bytes[2] |= cur[1];
731 bytes[3] |= cur[2];
732 cur += 3;
733 }
734 }
735
736 // Next Header
737 if ((hcCtl & kHcNextHeader) == 0)
738 {
739 VerifyOrExit(cur < end);
740 aIp6Header.SetNextHeader(cur[0]);
741 cur++;
742 aCompressedNextHeader = false;
743 }
744 else
745 {
746 aCompressedNextHeader = true;
747 }
748
749 // Hop Limit
750 switch (hcCtl & kHcHopLimitMask)
751 {
752 case kHcHopLimit1:
753 aIp6Header.SetHopLimit(1);
754 break;
755
756 case kHcHopLimit64:
757 aIp6Header.SetHopLimit(64);
758 break;
759
760 case kHcHopLimit255:
761 aIp6Header.SetHopLimit(255);
762 break;
763
764 default:
765 VerifyOrExit(cur < end);
766 aIp6Header.SetHopLimit(cur[0]);
767 cur++;
768 break;
769 }
770
771 // Source Address
772 switch (hcCtl & kHcSrcAddrModeMask)
773 {
774 case kHcSrcAddrMode0:
775 if ((hcCtl & kHcSrcAddrContext) == 0)
776 {
777 VerifyOrExit(cur + sizeof(Ip6::Address) <= end);
778 memcpy(&aIp6Header.GetSource(), cur, sizeof(aIp6Header.GetSource()));
779 cur += sizeof(Ip6::Address);
780 }
781
782 break;
783
784 case kHcSrcAddrMode1:
785 VerifyOrExit(cur + Ip6::InterfaceIdentifier::kSize <= end);
786 aIp6Header.GetSource().GetIid().SetBytes(cur);
787 cur += Ip6::InterfaceIdentifier::kSize;
788 break;
789
790 case kHcSrcAddrMode2:
791 VerifyOrExit(cur + 2 <= end);
792 aIp6Header.GetSource().mFields.m8[11] = 0xff;
793 aIp6Header.GetSource().mFields.m8[12] = 0xfe;
794 memcpy(aIp6Header.GetSource().mFields.m8 + 14, cur, 2);
795 cur += 2;
796 break;
797
798 case kHcSrcAddrMode3:
799 IgnoreError(ComputeIid(aMacSource, srcContext, aIp6Header.GetSource()));
800 break;
801 }
802
803 if ((hcCtl & kHcSrcAddrModeMask) != kHcSrcAddrMode0)
804 {
805 if ((hcCtl & kHcSrcAddrContext) == 0)
806 {
807 aIp6Header.GetSource().mFields.m16[0] = HostSwap16(0xfe80);
808 }
809 else
810 {
811 VerifyOrExit(srcContextValid);
812 CopyContext(srcContext, aIp6Header.GetSource());
813 }
814 }
815
816 if ((hcCtl & kHcMulticast) == 0)
817 {
818 // Unicast Destination Address
819
820 switch (hcCtl & kHcDstAddrModeMask)
821 {
822 case kHcDstAddrMode0:
823 VerifyOrExit((hcCtl & kHcDstAddrContext) == 0);
824 VerifyOrExit(cur + sizeof(Ip6::Address) <= end);
825 memcpy(&aIp6Header.GetDestination(), cur, sizeof(aIp6Header.GetDestination()));
826 cur += sizeof(Ip6::Address);
827 break;
828
829 case kHcDstAddrMode1:
830 VerifyOrExit(cur + Ip6::InterfaceIdentifier::kSize <= end);
831 aIp6Header.GetDestination().GetIid().SetBytes(cur);
832 cur += Ip6::InterfaceIdentifier::kSize;
833 break;
834
835 case kHcDstAddrMode2:
836 VerifyOrExit(cur + 2 <= end);
837 aIp6Header.GetDestination().mFields.m8[11] = 0xff;
838 aIp6Header.GetDestination().mFields.m8[12] = 0xfe;
839 memcpy(aIp6Header.GetDestination().mFields.m8 + 14, cur, 2);
840 cur += 2;
841 break;
842
843 case kHcDstAddrMode3:
844 SuccessOrExit(ComputeIid(aMacDest, dstContext, aIp6Header.GetDestination()));
845 break;
846 }
847
848 if ((hcCtl & kHcDstAddrContext) == 0)
849 {
850 if ((hcCtl & kHcDstAddrModeMask) != 0)
851 {
852 aIp6Header.GetDestination().mFields.m16[0] = HostSwap16(0xfe80);
853 }
854 }
855 else
856 {
857 VerifyOrExit(dstContextValid);
858 CopyContext(dstContext, aIp6Header.GetDestination());
859 }
860 }
861 else
862 {
863 // Multicast Destination Address
864
865 aIp6Header.GetDestination().mFields.m8[0] = 0xff;
866
867 if ((hcCtl & kHcDstAddrContext) == 0)
868 {
869 switch (hcCtl & kHcDstAddrModeMask)
870 {
871 case kHcDstAddrMode0:
872 VerifyOrExit(cur + sizeof(Ip6::Address) <= end);
873 memcpy(aIp6Header.GetDestination().mFields.m8, cur, sizeof(Ip6::Address));
874 cur += sizeof(Ip6::Address);
875 break;
876
877 case kHcDstAddrMode1:
878 VerifyOrExit(cur + 6 <= end);
879 aIp6Header.GetDestination().mFields.m8[1] = cur[0];
880 memcpy(aIp6Header.GetDestination().mFields.m8 + 11, cur + 1, 5);
881 cur += 6;
882 break;
883
884 case kHcDstAddrMode2:
885 VerifyOrExit(cur + 4 <= end);
886 aIp6Header.GetDestination().mFields.m8[1] = cur[0];
887 memcpy(aIp6Header.GetDestination().mFields.m8 + 13, cur + 1, 3);
888 cur += 4;
889 break;
890
891 case kHcDstAddrMode3:
892 VerifyOrExit(cur < end);
893 aIp6Header.GetDestination().mFields.m8[1] = 0x02;
894 aIp6Header.GetDestination().mFields.m8[15] = cur[0];
895 cur++;
896 break;
897 }
898 }
899 else
900 {
901 switch (hcCtl & kHcDstAddrModeMask)
902 {
903 case 0:
904 VerifyOrExit(cur + 6 <= end);
905 VerifyOrExit(dstContextValid);
906 aIp6Header.GetDestination().mFields.m8[1] = cur[0];
907 aIp6Header.GetDestination().mFields.m8[2] = cur[1];
908 aIp6Header.GetDestination().mFields.m8[3] = dstContext.mPrefix.GetLength();
909 memcpy(aIp6Header.GetDestination().mFields.m8 + 4, dstContext.mPrefix.GetBytes(), 8);
910 memcpy(aIp6Header.GetDestination().mFields.m8 + 12, cur + 2, 4);
911 cur += 6;
912 break;
913
914 default:
915 ExitNow();
916 }
917 }
918 }
919
920 if ((hcCtl & kHcNextHeader) != 0)
921 {
922 VerifyOrExit(cur < end);
923 SuccessOrExit(DispatchToNextHeader(cur[0], nextHeader));
924 aIp6Header.SetNextHeader(nextHeader);
925 }
926
927 error = kErrorNone;
928
929 exit:
930 return (error == kErrorNone) ? static_cast<int>(cur - aBuf) : -1;
931 }
932
DecompressExtensionHeader(Message & aMessage,const uint8_t * aBuf,uint16_t aBufLength)933 int Lowpan::DecompressExtensionHeader(Message &aMessage, const uint8_t *aBuf, uint16_t aBufLength)
934 {
935 Error error = kErrorParse;
936 const uint8_t * cur = aBuf;
937 const uint8_t * end = aBuf + aBufLength;
938 uint8_t hdr[2];
939 uint8_t len;
940 uint8_t nextHeader;
941 uint8_t ctl = cur[0];
942 uint8_t padLength;
943 Ip6::OptionPad1 optionPad1;
944 Ip6::OptionPadN optionPadN;
945
946 VerifyOrExit(cur < end);
947 cur++;
948
949 // next header
950 if (ctl & kExtHdrNextHeader)
951 {
952 VerifyOrExit(cur < end);
953
954 len = cur[0];
955 cur++;
956
957 VerifyOrExit(cur + len <= end);
958 SuccessOrExit(DispatchToNextHeader(cur[len], nextHeader));
959 hdr[0] = static_cast<uint8_t>(nextHeader);
960 }
961 else
962 {
963 VerifyOrExit(cur + 2 <= end);
964
965 hdr[0] = cur[0];
966 len = cur[1];
967 cur += 2;
968
969 VerifyOrExit(cur + len <= end);
970 }
971
972 // length
973 hdr[1] = BitVectorBytes(sizeof(hdr) + len) - 1;
974
975 SuccessOrExit(aMessage.AppendBytes(hdr, sizeof(hdr)));
976 aMessage.MoveOffset(sizeof(hdr));
977
978 // payload
979 SuccessOrExit(aMessage.AppendBytes(cur, len));
980 aMessage.MoveOffset(len);
981 cur += len;
982
983 // The RFC6282 says: "The trailing Pad1 or PadN option MAY be elided by the compressor.
984 // A decompressor MUST ensure that the containing header is padded out to a multiple of 8 octets
985 // in length, using a Pad1 or PadN option if necessary."
986 padLength = 8 - ((len + sizeof(hdr)) & 0x07);
987
988 if (padLength != 8)
989 {
990 if (padLength == 1)
991 {
992 optionPad1.Init();
993 SuccessOrExit(aMessage.AppendBytes(&optionPad1, padLength));
994 }
995 else
996 {
997 optionPadN.Init(padLength);
998 SuccessOrExit(aMessage.AppendBytes(&optionPadN, padLength));
999 }
1000
1001 aMessage.MoveOffset(padLength);
1002 }
1003
1004 error = kErrorNone;
1005
1006 exit:
1007 return (error == kErrorNone) ? static_cast<int>(cur - aBuf) : -1;
1008 }
1009
DecompressUdpHeader(Ip6::Udp::Header & aUdpHeader,const uint8_t * aBuf,uint16_t aBufLength)1010 int Lowpan::DecompressUdpHeader(Ip6::Udp::Header &aUdpHeader, const uint8_t *aBuf, uint16_t aBufLength)
1011 {
1012 Error error = kErrorParse;
1013 const uint8_t *cur = aBuf;
1014 const uint8_t *end = aBuf + aBufLength;
1015 uint8_t udpCtl;
1016
1017 VerifyOrExit(cur < end);
1018 udpCtl = cur[0];
1019 cur++;
1020
1021 VerifyOrExit((udpCtl & kUdpDispatchMask) == kUdpDispatch);
1022
1023 memset(&aUdpHeader, 0, sizeof(aUdpHeader));
1024
1025 // source and dest ports
1026 switch (udpCtl & kUdpPortMask)
1027 {
1028 case 0:
1029 VerifyOrExit(cur + 4 <= end);
1030 aUdpHeader.SetSourcePort(ReadUint16(cur));
1031 aUdpHeader.SetDestinationPort(ReadUint16(cur + 2));
1032 cur += 4;
1033 break;
1034
1035 case 1:
1036 VerifyOrExit(cur + 3 <= end);
1037 aUdpHeader.SetSourcePort(ReadUint16(cur));
1038 aUdpHeader.SetDestinationPort(0xf000 | cur[2]);
1039 cur += 3;
1040 break;
1041
1042 case 2:
1043 VerifyOrExit(cur + 3 <= end);
1044 aUdpHeader.SetSourcePort(0xf000 | cur[0]);
1045 aUdpHeader.SetDestinationPort(ReadUint16(cur + 1));
1046 cur += 3;
1047 break;
1048
1049 case 3:
1050 VerifyOrExit(cur < end);
1051 aUdpHeader.SetSourcePort(0xf0b0 | (cur[0] >> 4));
1052 aUdpHeader.SetDestinationPort(0xf0b0 | (cur[0] & 0xf));
1053 cur++;
1054 break;
1055 }
1056
1057 // checksum
1058 if ((udpCtl & kUdpChecksum) != 0)
1059 {
1060 ExitNow();
1061 }
1062 else
1063 {
1064 VerifyOrExit(cur + 2 <= end);
1065 aUdpHeader.SetChecksum(ReadUint16(cur));
1066 cur += 2;
1067 }
1068
1069 error = kErrorNone;
1070
1071 exit:
1072 return (error == kErrorNone) ? static_cast<int>(cur - aBuf) : -1;
1073 }
1074
DecompressUdpHeader(Message & aMessage,const uint8_t * aBuf,uint16_t aBufLength,uint16_t aDatagramLength)1075 int Lowpan::DecompressUdpHeader(Message &aMessage, const uint8_t *aBuf, uint16_t aBufLength, uint16_t aDatagramLength)
1076 {
1077 Ip6::Udp::Header udpHeader;
1078 int headerLen = -1;
1079
1080 headerLen = DecompressUdpHeader(udpHeader, aBuf, aBufLength);
1081 VerifyOrExit(headerLen >= 0);
1082
1083 // length
1084 if (aDatagramLength == 0)
1085 {
1086 udpHeader.SetLength(sizeof(udpHeader) + static_cast<uint16_t>(aBufLength - headerLen));
1087 }
1088 else
1089 {
1090 udpHeader.SetLength(aDatagramLength - aMessage.GetOffset());
1091 }
1092
1093 VerifyOrExit(aMessage.Append(udpHeader) == kErrorNone, headerLen = -1);
1094 aMessage.MoveOffset(sizeof(udpHeader));
1095
1096 exit:
1097 return headerLen;
1098 }
1099
Decompress(Message & aMessage,const Mac::Address & aMacSource,const Mac::Address & aMacDest,const uint8_t * aBuf,uint16_t aBufLength,uint16_t aDatagramLength)1100 int Lowpan::Decompress(Message & aMessage,
1101 const Mac::Address &aMacSource,
1102 const Mac::Address &aMacDest,
1103 const uint8_t * aBuf,
1104 uint16_t aBufLength,
1105 uint16_t aDatagramLength)
1106 {
1107 Error error = kErrorParse;
1108 Ip6::Header ip6Header;
1109 const uint8_t *cur = aBuf;
1110 uint16_t remaining = aBufLength;
1111 bool compressed;
1112 int rval;
1113 uint16_t ip6PayloadLength;
1114 uint16_t compressedLength = 0;
1115 uint16_t currentOffset = aMessage.GetOffset();
1116
1117 VerifyOrExit(remaining >= 2);
1118 VerifyOrExit((rval = DecompressBaseHeader(ip6Header, compressed, aMacSource, aMacDest, cur, remaining)) >= 0);
1119
1120 cur += rval;
1121 remaining -= rval;
1122
1123 SuccessOrExit(aMessage.Append(ip6Header));
1124 aMessage.MoveOffset(sizeof(ip6Header));
1125
1126 while (compressed)
1127 {
1128 VerifyOrExit(remaining >= 1);
1129
1130 if ((cur[0] & kExtHdrDispatchMask) == kExtHdrDispatch)
1131 {
1132 if ((cur[0] & kExtHdrEidMask) == kExtHdrEidIp6)
1133 {
1134 compressed = false;
1135
1136 cur++;
1137 remaining--;
1138
1139 VerifyOrExit((rval = Decompress(aMessage, aMacSource, aMacDest, cur, remaining, aDatagramLength)) >= 0);
1140 }
1141 else
1142 {
1143 compressed = (cur[0] & kExtHdrNextHeader) != 0;
1144 VerifyOrExit((rval = DecompressExtensionHeader(aMessage, cur, remaining)) >= 0);
1145 }
1146 }
1147 else if ((cur[0] & kUdpDispatchMask) == kUdpDispatch)
1148 {
1149 compressed = false;
1150 VerifyOrExit((rval = DecompressUdpHeader(aMessage, cur, remaining, aDatagramLength)) >= 0);
1151 }
1152 else
1153 {
1154 ExitNow();
1155 }
1156
1157 VerifyOrExit(remaining >= rval);
1158 cur += rval;
1159 remaining -= rval;
1160 }
1161
1162 compressedLength = static_cast<uint16_t>(cur - aBuf);
1163
1164 if (aDatagramLength)
1165 {
1166 ip6PayloadLength = HostSwap16(aDatagramLength - currentOffset - sizeof(Ip6::Header));
1167 }
1168 else
1169 {
1170 ip6PayloadLength =
1171 HostSwap16(aMessage.GetOffset() - currentOffset - sizeof(Ip6::Header) + aBufLength - compressedLength);
1172 }
1173
1174 aMessage.Write(currentOffset + Ip6::Header::kPayloadLengthFieldOffset, ip6PayloadLength);
1175
1176 error = kErrorNone;
1177
1178 exit:
1179 return (error == kErrorNone) ? static_cast<int>(compressedLength) : -1;
1180 }
1181
1182 //---------------------------------------------------------------------------------------------------------------------
1183 // MeshHeader
1184
Init(uint16_t aSource,uint16_t aDestination,uint8_t aHopsLeft)1185 void MeshHeader::Init(uint16_t aSource, uint16_t aDestination, uint8_t aHopsLeft)
1186 {
1187 mSource = aSource;
1188 mDestination = aDestination;
1189 mHopsLeft = aHopsLeft;
1190 }
1191
IsMeshHeader(const uint8_t * aFrame,uint16_t aFrameLength)1192 bool MeshHeader::IsMeshHeader(const uint8_t *aFrame, uint16_t aFrameLength)
1193 {
1194 return (aFrameLength >= kMinHeaderLength) && ((*aFrame & kDispatchMask) == kDispatch);
1195 }
1196
ParseFrom(const uint8_t * aFrame,uint16_t aFrameLength,uint16_t & aHeaderLength)1197 Error MeshHeader::ParseFrom(const uint8_t *aFrame, uint16_t aFrameLength, uint16_t &aHeaderLength)
1198 {
1199 Error error = kErrorParse;
1200 uint8_t dispatch;
1201
1202 VerifyOrExit(aFrameLength >= kMinHeaderLength);
1203 dispatch = *aFrame++;
1204
1205 VerifyOrExit((dispatch & (kDispatchMask | kSourceShort | kDestShort)) == (kDispatch | kSourceShort | kDestShort));
1206
1207 mHopsLeft = (dispatch & kHopsLeftMask);
1208
1209 if (mHopsLeft == kDeepHopsLeft)
1210 {
1211 VerifyOrExit(aFrameLength >= kDeepHopsHeaderLength);
1212 mHopsLeft = *aFrame++;
1213 aHeaderLength = kDeepHopsHeaderLength;
1214 }
1215 else
1216 {
1217 aHeaderLength = kMinHeaderLength;
1218 }
1219
1220 mSource = ReadUint16(aFrame);
1221 mDestination = ReadUint16(aFrame + 2);
1222
1223 error = kErrorNone;
1224
1225 exit:
1226 return error;
1227 }
1228
ParseFrom(const Message & aMessage)1229 Error MeshHeader::ParseFrom(const Message &aMessage)
1230 {
1231 uint16_t headerLength;
1232
1233 return ParseFrom(aMessage, headerLength);
1234 }
1235
ParseFrom(const Message & aMessage,uint16_t & aHeaderLength)1236 Error MeshHeader::ParseFrom(const Message &aMessage, uint16_t &aHeaderLength)
1237 {
1238 uint8_t frame[kDeepHopsHeaderLength];
1239 uint16_t frameLength;
1240
1241 frameLength = aMessage.ReadBytes(/* aOffset */ 0, frame, sizeof(frame));
1242
1243 return ParseFrom(frame, frameLength, aHeaderLength);
1244 }
1245
GetHeaderLength(void) const1246 uint16_t MeshHeader::GetHeaderLength(void) const
1247 {
1248 return (mHopsLeft >= kDeepHopsLeft) ? kDeepHopsHeaderLength : kMinHeaderLength;
1249 }
1250
DecrementHopsLeft(void)1251 void MeshHeader::DecrementHopsLeft(void)
1252 {
1253 if (mHopsLeft > 0)
1254 {
1255 mHopsLeft--;
1256 }
1257 }
1258
WriteTo(uint8_t * aFrame) const1259 uint16_t MeshHeader::WriteTo(uint8_t *aFrame) const
1260 {
1261 uint8_t *cur = aFrame;
1262 uint8_t dispatch = (kDispatch | kSourceShort | kDestShort);
1263
1264 if (mHopsLeft < kDeepHopsLeft)
1265 {
1266 *cur++ = (dispatch | mHopsLeft);
1267 }
1268 else
1269 {
1270 *cur++ = (dispatch | kDeepHopsLeft);
1271 *cur++ = mHopsLeft;
1272 }
1273
1274 WriteUint16(mSource, cur);
1275 cur += sizeof(uint16_t);
1276
1277 WriteUint16(mDestination, cur);
1278 cur += sizeof(uint16_t);
1279
1280 return static_cast<uint16_t>(cur - aFrame);
1281 }
1282
WriteTo(Message & aMessage,uint16_t aOffset) const1283 uint16_t MeshHeader::WriteTo(Message &aMessage, uint16_t aOffset) const
1284 {
1285 uint8_t frame[kDeepHopsHeaderLength];
1286 uint16_t headerLength;
1287
1288 headerLength = WriteTo(frame);
1289 aMessage.WriteBytes(aOffset, frame, headerLength);
1290
1291 return headerLength;
1292 }
1293
1294 //---------------------------------------------------------------------------------------------------------------------
1295 // FragmentHeader
1296
Init(uint16_t aSize,uint16_t aTag,uint16_t aOffset)1297 void FragmentHeader::Init(uint16_t aSize, uint16_t aTag, uint16_t aOffset)
1298 {
1299 mSize = (aSize & kSizeMask);
1300 mTag = aTag;
1301 mOffset = (aOffset & kOffsetMask);
1302 }
1303
IsFragmentHeader(const uint8_t * aFrame,uint16_t aFrameLength)1304 bool FragmentHeader::IsFragmentHeader(const uint8_t *aFrame, uint16_t aFrameLength)
1305 {
1306 return (aFrameLength >= kFirstFragmentHeaderSize) && ((*aFrame & kDispatchMask) == kDispatch);
1307 }
1308
ParseFrom(const uint8_t * aFrame,uint16_t aFrameLength,uint16_t & aHeaderLength)1309 Error FragmentHeader::ParseFrom(const uint8_t *aFrame, uint16_t aFrameLength, uint16_t &aHeaderLength)
1310 {
1311 Error error = kErrorParse;
1312
1313 VerifyOrExit(IsFragmentHeader(aFrame, aFrameLength));
1314
1315 mSize = ReadUint16(aFrame + kSizeIndex) & kSizeMask;
1316 mTag = ReadUint16(aFrame + kTagIndex);
1317
1318 if ((*aFrame & kOffsetFlag) == kOffsetFlag)
1319 {
1320 VerifyOrExit(aFrameLength >= kSubsequentFragmentHeaderSize);
1321 mOffset = aFrame[kOffsetIndex] * 8;
1322 aHeaderLength = kSubsequentFragmentHeaderSize;
1323 }
1324 else
1325 {
1326 mOffset = 0;
1327 aHeaderLength = kFirstFragmentHeaderSize;
1328 }
1329
1330 error = kErrorNone;
1331
1332 exit:
1333 return error;
1334 }
1335
ParseFrom(const Message & aMessage,uint16_t aOffset,uint16_t & aHeaderLength)1336 Error FragmentHeader::ParseFrom(const Message &aMessage, uint16_t aOffset, uint16_t &aHeaderLength)
1337 {
1338 uint8_t frame[kSubsequentFragmentHeaderSize];
1339 uint16_t frameLength;
1340
1341 frameLength = aMessage.ReadBytes(aOffset, frame, sizeof(frame));
1342
1343 return ParseFrom(frame, frameLength, aHeaderLength);
1344 }
1345
WriteTo(uint8_t * aFrame) const1346 uint16_t FragmentHeader::WriteTo(uint8_t *aFrame) const
1347 {
1348 uint8_t *cur = aFrame;
1349
1350 WriteUint16((static_cast<uint16_t>(kDispatch) << 8) + mSize, cur);
1351 cur += sizeof(uint16_t);
1352
1353 WriteUint16(mTag, cur);
1354 cur += sizeof(uint16_t);
1355
1356 if (mOffset != 0)
1357 {
1358 *aFrame |= kOffsetFlag;
1359 *cur++ = static_cast<uint8_t>(mOffset >> 3);
1360 }
1361
1362 return static_cast<uint16_t>(cur - aFrame);
1363 }
1364
1365 } // namespace Lowpan
1366 } // namespace ot
1367
