src/core/thread/lowpan.cpp

/*
 *  Copyright (c) 2016, The OpenThread Authors.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of the copyright holder nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @file
 *   This file implements 6LoWPAN header compression.
 */

#include <common/code_utils.hpp>
#include <common/debug.hpp>
#include <common/encoding.hpp>
#include <net/ip6.hpp>
#include <net/udp6.hpp>
#include <thread/lowpan.hpp>
#include <thread/network_data_leader.hpp>
#include <thread/thread_netif.hpp>

using Thread::Encoding::BigEndian::HostSwap16;

namespace Thread {
namespace Lowpan {

Lowpan::Lowpan(ThreadNetif &aThreadNetif):
    mNetworkData(aThreadNetif.GetNetworkDataLeader())
{
}

ThreadError Lowpan::CopyContext(const Context &aContext, Ip6::Address &aAddress)
{
    memcpy(&aAddress, aContext.mPrefix, aContext.mPrefixLength / CHAR_BIT);

    for (int i = (aContext.mPrefixLength & ~7); i < aContext.mPrefixLength; i++)
    {
        aAddress.mFields.m8[i / CHAR_BIT] &= ~(0x80 >> (i % CHAR_BIT));
        aAddress.mFields.m8[i / CHAR_BIT] |= aContext.mPrefix[i / CHAR_BIT] & (0x80 >> (i % CHAR_BIT));
    }

    return kThreadError_None;
}

ThreadError Lowpan::ComputeIid(const Mac::Address &aMacAddr, const Context &aContext, Ip6::Address &aIpAddress)
{
    switch (aMacAddr.mLength)
    {
    case 2:
        aIpAddress.mFields.m16[4] = HostSwap16(0x0000);
        aIpAddress.mFields.m16[5] = HostSwap16(0x00ff);
        aIpAddress.mFields.m16[6] = HostSwap16(0xfe00);
        aIpAddress.mFields.m16[7] = HostSwap16(aMacAddr.mShortAddress);
        break;

    case Ip6::Address::kInterfaceIdentifierSize:
        aIpAddress.SetIid(aMacAddr.mExtAddress);
        break;

    default:
        assert(false);
    }

    if (aContext.mPrefixLength > 64)
    {
        for (int i = (aContext.mPrefixLength & ~7); i < aContext.mPrefixLength; i++)
        {
            aIpAddress.mFields.m8[i / CHAR_BIT] &= ~(0x80 >> (i % CHAR_BIT));
            aIpAddress.mFields.m8[i / CHAR_BIT] |= aContext.mPrefix[i / CHAR_BIT] & (0x80 >> (i % CHAR_BIT));
        }
    }

    return kThreadError_None;
}

int Lowpan::CompressSourceIid(const Mac::Address &aMacAddr, const Ip6::Address &aIpAddr, const Context &aContext,
                              uint16_t &aHcCtl, uint8_t *aBuf)
{
    uint8_t *cur = aBuf;
    Ip6::Address ipaddr;
    Mac::Address tmp;

    ComputeIid(aMacAddr, aContext, ipaddr);

    if (memcmp(ipaddr.GetIid(), aIpAddr.GetIid(), Ip6::Address::kInterfaceIdentifierSize) == 0)
    {
        aHcCtl |= kHcSrcAddrMode3;
    }
    else
    {
        tmp.mLength = sizeof(tmp.mShortAddress);
        tmp.mShortAddress = HostSwap16(aIpAddr.mFields.m16[7]);
        ComputeIid(tmp, aContext, ipaddr);

        if (memcmp(ipaddr.GetIid(), aIpAddr.GetIid(), Ip6::Address::kInterfaceIdentifierSize) == 0)
        {
            aHcCtl |= kHcSrcAddrMode2;
            cur[0] = aIpAddr.mFields.m8[14];
            cur[1] = aIpAddr.mFields.m8[15];
            cur += 2;
        }
        else
        {
            aHcCtl |= kHcSrcAddrMode1;
            memcpy(cur, aIpAddr.GetIid(), Ip6::Address::kInterfaceIdentifierSize);
            cur += Ip6::Address::kInterfaceIdentifierSize;
        }
    }

    return static_cast<int>(cur - aBuf);
}

int Lowpan::CompressDestinationIid(const Mac::Address &aMacAddr, const Ip6::Address &aIpAddr, const Context &aContext,
                                   uint16_t &aHcCtl, uint8_t *aBuf)
{
    uint8_t *cur = aBuf;
    Ip6::Address ipaddr;
    Mac::Address tmp;

    ComputeIid(aMacAddr, aContext, ipaddr);

    if (memcmp(ipaddr.GetIid(), aIpAddr.GetIid(), Ip6::Address::kInterfaceIdentifierSize) == 0)
    {
        aHcCtl |= kHcDstAddrMode3;
    }
    else
    {
        tmp.mLength = sizeof(tmp.mShortAddress);
        tmp.mShortAddress = HostSwap16(aIpAddr.mFields.m16[7]);
        ComputeIid(tmp, aContext, ipaddr);

        if (memcmp(ipaddr.GetIid(), aIpAddr.GetIid(), Ip6::Address::kInterfaceIdentifierSize) == 0)
        {
            aHcCtl |= kHcDstAddrMode2;
            cur[0] = aIpAddr.mFields.m8[14];
            cur[1] = aIpAddr.mFields.m8[15];
            cur += 2;
        }
        else
        {
            aHcCtl |= kHcDstAddrMode1;
            memcpy(cur, aIpAddr.GetIid(), Ip6::Address::kInterfaceIdentifierSize);
            cur += Ip6::Address::kInterfaceIdentifierSize;
        }
    }

    return static_cast<int>(cur - aBuf);
}

int Lowpan::CompressMulticast(const Ip6::Address &aIpAddr, uint16_t &aHcCtl, uint8_t *aBuf)
{
    uint8_t *cur = aBuf;

    aHcCtl |= kHcMulticast;

    for (unsigned int i = 2; i < sizeof(Ip6::Address); i++)
    {
        if (aIpAddr.mFields.m8[i])
        {
            if (aIpAddr.mFields.m8[1] == 0x02 && i >= 15)
            {
                aHcCtl |= kHcDstAddrMode3;
                cur[0] = aIpAddr.mFields.m8[15];
                cur++;
            }
            else if (i >= 13)
            {
                aHcCtl |= kHcDstAddrMode2;
                cur[0] = aIpAddr.mFields.m8[1];
                memcpy(cur + 1, aIpAddr.mFields.m8 + 13, 3);
                cur += 4;
            }
            else if (i >= 9)
            {
                aHcCtl |= kHcDstAddrMode1;
                cur[0] = aIpAddr.mFields.m8[1];
                memcpy(cur + 1, aIpAddr.mFields.m8 + 11, 5);
                cur += 6;
            }
            else
            {
                memcpy(cur, aIpAddr.mFields.m8, sizeof(Ip6::Address));
                cur += sizeof(Ip6::Address);
            }

            break;
        }
    }

    return static_cast<int>(cur - aBuf);
}

int Lowpan::Compress(Message &aMessage, const Mac::Address &aMacSource, const Mac::Address &aMacDest, uint8_t *aBuf)
{
    uint8_t *cur = aBuf;
    uint16_t hcCtl = 0;
    Ip6::Header ip6Header;
    uint8_t *ip6HeaderBytes = reinterpret_cast<uint8_t *>(&ip6Header);
    Context srcContext, dstContext;
    bool srcContextValid = true, dstContextValid = true;
    uint8_t nextHeader;

    aMessage.Read(0, sizeof(ip6Header), &ip6Header);

    if (mNetworkData.GetContext(ip6Header.GetSource(), srcContext) != kThreadError_None)
    {
        mNetworkData.GetContext(0, srcContext);
        srcContextValid = false;
    }

    if (mNetworkData.GetContext(ip6Header.GetDestination(), dstContext) != kThreadError_None)
    {
        mNetworkData.GetContext(0, dstContext);
        dstContextValid = false;
    }

    hcCtl = kHcDispatch;

    // Lowpan HC Control Bits
    cur += 2;

    // Context Identifier
    if (srcContext.mContextId != 0 || dstContext.mContextId != 0)
    {
        hcCtl |= kHcContextId;
        cur[0] = ((srcContext.mContextId << 4) | dstContext.mContextId) & 0xff;
        cur++;
    }

    // Traffic Class
    if (((ip6HeaderBytes[0] & 0x0f) == 0) && ((ip6HeaderBytes[1] & 0xf0) == 0))
    {
        hcCtl |= kHcTrafficClass;
    }

    // Flow Label
    if (((ip6HeaderBytes[1] & 0x0f) == 0) && ((ip6HeaderBytes[2]) == 0) && ((ip6HeaderBytes[3]) == 0))
    {
        hcCtl |= kHcFlowLabel;
    }

    if ((hcCtl & kHcTrafficFlowMask) != kHcTrafficFlow)
    {
        cur[0] = ((ip6HeaderBytes[1] >> 4) << 6) & 0xff;

        if ((hcCtl & kHcTrafficClass) == 0)
        {
            cur[0] |= ((ip6HeaderBytes[0] & 0x0f) << 2) | (ip6HeaderBytes[1] >> 6);
            cur++;
        }

        if ((hcCtl & kHcFlowLabel) == 0)
        {
            cur[0] |= ip6HeaderBytes[1] & 0x0f;
            cur[1] = ip6HeaderBytes[2];
            cur[2] = ip6HeaderBytes[3];
            cur += 3;
        }
    }

    // Next Header
    switch (ip6Header.GetNextHeader())
    {
    case Ip6::kProtoHopOpts:
    case Ip6::kProtoUdp:
        hcCtl |= kHcNextHeader;
        break;

    default:
        cur[0] = static_cast<uint8_t>(ip6Header.GetNextHeader());
        cur++;
        break;
    }

    // Hop Limit
    switch (ip6Header.GetHopLimit())
    {
    case 1:
        hcCtl |= kHcHopLimit1;
        break;

    case 64:
        hcCtl |= kHcHopLimit64;
        break;

    case 255:
        hcCtl |= kHcHopLimit255;
        break;

    default:
        cur[0] = ip6Header.GetHopLimit();
        cur++;
        break;
    }

    // Source Address
    if (ip6Header.GetSource().IsUnspecified())
    {
        hcCtl |= kHcSrcAddrContext;
    }
    else if (ip6Header.GetSource().IsLinkLocal())
    {
        cur += CompressSourceIid(aMacSource, ip6Header.GetSource(), srcContext, hcCtl, cur);
    }
    else if (srcContextValid)
    {
        hcCtl |= kHcSrcAddrContext;
        cur += CompressSourceIid(aMacSource, ip6Header.GetSource(), srcContext, hcCtl, cur);
    }
    else
    {
        memcpy(cur, ip6Header.GetSource().mFields.m8, sizeof(ip6Header.GetSource()));
        cur += sizeof(Ip6::Address);
    }

    // Destination Address
    if (ip6Header.GetDestination().IsMulticast())
    {
        cur += CompressMulticast(ip6Header.GetDestination(), hcCtl, cur);
    }
    else if (ip6Header.GetDestination().IsLinkLocal())
    {
        cur += CompressDestinationIid(aMacDest, ip6Header.GetDestination(), dstContext, hcCtl, cur);
    }
    else if (dstContextValid)
    {
        hcCtl |= kHcDstAddrContext;
        cur += CompressDestinationIid(aMacDest, ip6Header.GetDestination(), dstContext, hcCtl, cur);
    }
    else
    {
        memcpy(cur, &ip6Header.GetDestination(), sizeof(ip6Header.GetDestination()));
        cur += sizeof(Ip6::Address);
    }

    aBuf[0] = hcCtl >> 8;
    aBuf[1] = hcCtl & 0xff;
    aMessage.SetOffset(sizeof(ip6Header));

    nextHeader = static_cast<uint8_t>(ip6Header.GetNextHeader());

    while (1)
    {
        switch (nextHeader)
        {
        case Ip6::kProtoHopOpts:
            cur += CompressExtensionHeader(aMessage, cur, nextHeader);
            break;

        case Ip6::kProtoUdp:
            cur += CompressUdp(aMessage, cur);
            ExitNow();

        default:
            ExitNow();
        }
    }

exit:
    return static_cast<int>(cur - aBuf);
}

int Lowpan::CompressExtensionHeader(Message &aMessage, uint8_t *aBuf, uint8_t &aNextHeader)
{
    Ip6::ExtensionHeader extHeader;
    uint8_t *cur = aBuf;
    uint8_t len;

    aMessage.Read(aMessage.GetOffset(), sizeof(extHeader), &extHeader);
    aMessage.MoveOffset(sizeof(extHeader));

    cur[0] = kExtHdrDispatch | kExtHdrEidHbh;
    aNextHeader = static_cast<uint8_t>(extHeader.GetNextHeader());

    switch (extHeader.GetNextHeader())
    {
    case Ip6::kProtoUdp:
        cur[0] |= kExtHdrNextHeader;
        break;

    default:
        cur++;
        cur[0] = static_cast<uint8_t>(extHeader.GetNextHeader());
        break;
    }

    cur++;

    len = (extHeader.GetLength() + 1) * 8 - sizeof(extHeader);
    cur[0] = len;
    cur++;

    aMessage.Read(aMessage.GetOffset(), len, cur);
    aMessage.MoveOffset(len);
    cur += len;

    return static_cast<int>(cur - aBuf);
}

int Lowpan::CompressUdp(Message &aMessage, uint8_t *aBuf)
{
    Ip6::UdpHeader udpHeader;
    uint8_t *cur = aBuf;
    uint8_t *udpCtl = cur;
    uint16_t source;
    uint16_t destination;

    aMessage.Read(aMessage.GetOffset(), sizeof(udpHeader), &udpHeader);
    source = udpHeader.GetSourcePort();
    destination = udpHeader.GetDestinationPort();

    cur[0] = kUdpDispatch;
    cur++;

    if ((source & 0xfff0) == 0xf0b0 && (destination & 0xfff0) == 0xf0b0)
    {
        *udpCtl |= 3;
        *cur++ = (((source & 0xf) << 4) | (destination & 0xf)) & 0xff;
    }
    else if ((source & 0xff00) == 0xf000)
    {
        *udpCtl |= 2;
        *cur++ = source & 0xff;
        *cur++ = destination >> 8;
        *cur++ = destination & 0xff;
    }
    else if ((destination & 0xff00) == 0xf000)
    {
        *udpCtl |= 1;
        *cur++ = source >> 8;
        *cur++ = source & 0xff;
        *cur++ = destination & 0xff;
    }
    else
    {
        memcpy(cur, &udpHeader, Ip6::UdpHeader::GetLengthOffset());
        cur += Ip6::UdpHeader::GetLengthOffset();
    }

    memcpy(cur, reinterpret_cast<uint8_t *>(&udpHeader) + Ip6::UdpHeader::GetChecksumOffset(), 2);
    cur += 2;

    aMessage.MoveOffset(sizeof(udpHeader));

    return static_cast<int>(cur - aBuf);
}

ThreadError Lowpan::DispatchToNextHeader(uint8_t aDispatch, Ip6::IpProto &aNextHeader)
{
    ThreadError error = kThreadError_None;

    if ((aDispatch & kExtHdrDispatchMask) == kExtHdrDispatch)
    {
        switch (aDispatch & kExtHdrEidMask)
        {
        case kExtHdrEidHbh:
            aNextHeader = Ip6::kProtoHopOpts;
            ExitNow();

        case kExtHdrEidRouting:
            aNextHeader = Ip6::kProtoRouting;
            ExitNow();

        case kExtHdrEidFragment:
            aNextHeader = Ip6::kProtoFragment;
            ExitNow();

        case kExtHdrEidDst:
            aNextHeader = Ip6::kProtoDstOpts;
            ExitNow();

        case kExtHdrEidIp6:
            aNextHeader = Ip6::kProtoIp6;
            ExitNow();
        }
    }
    else if ((aDispatch & kUdpDispatchMask) == kUdpDispatch)
    {
        aNextHeader = Ip6::kProtoUdp;
        ExitNow();
    }

    error = kThreadError_Parse;

exit:
    return error;
}

int Lowpan::DecompressBaseHeader(Ip6::Header &ip6Header, const Mac::Address &aMacSource, const Mac::Address &aMacDest,
                                 const uint8_t *aBuf)
{
    ThreadError error = kThreadError_None;
    const uint8_t *cur = aBuf;
    uint16_t hcCtl;
    Context srcContext, dstContext;
    bool srcContextValid = true, dstContextValid = true;
    Ip6::IpProto nextHeader;
    uint8_t *bytes;

    hcCtl = static_cast<uint16_t>((cur[0] << 8) | cur[1]);
    cur += 2;

    // check Dispatch bits
    VerifyOrExit((hcCtl & kHcDispatchMask) == kHcDispatch, error = kThreadError_Parse);

    // Context Identifier
    srcContext.mPrefixLength = 0;
    dstContext.mPrefixLength = 0;

    if ((hcCtl & kHcContextId) != 0)
    {
        if (mNetworkData.GetContext(cur[0] >> 4, srcContext) != kThreadError_None)
        {
            srcContextValid = false;
        }

        if (mNetworkData.GetContext(cur[0] & 0xf, dstContext) != kThreadError_None)
        {
            dstContextValid = false;
        }

        cur++;
    }
    else
    {
        mNetworkData.GetContext(0, srcContext);
        mNetworkData.GetContext(0, dstContext);
    }

    memset(&ip6Header, 0, sizeof(ip6Header));
    ip6Header.Init();

    // Traffic Class and Flow Label
    if ((hcCtl & kHcTrafficFlowMask) != kHcTrafficFlow)
    {
        bytes = reinterpret_cast<uint8_t *>(&ip6Header);
        bytes[1] |= (cur[0] & 0xc0) >> 2;

        if ((hcCtl & kHcTrafficClass) == 0)
        {
            bytes[0] |= (cur[0] >> 2) & 0x0f;
            bytes[1] |= (cur[0] << 6) & 0xc0;
            cur++;
        }

        if ((hcCtl & kHcFlowLabel) == 0)
        {
            bytes[1] |= cur[0] & 0x0f;
            bytes[2] |= cur[1];
            bytes[3] |= cur[2];
            cur += 3;
        }
    }

    // Next Header
    if ((hcCtl & kHcNextHeader) == 0)
    {
        ip6Header.SetNextHeader(static_cast<Ip6::IpProto>(cur[0]));
        cur++;
    }

    // Hop Limit
    switch (hcCtl & kHcHopLimitMask)
    {
    case kHcHopLimit1:
        ip6Header.SetHopLimit(1);
        break;

    case kHcHopLimit64:
        ip6Header.SetHopLimit(64);
        break;

    case kHcHopLimit255:
        ip6Header.SetHopLimit(255);
        break;

    default:
        ip6Header.SetHopLimit(cur[0]);
        cur++;
        break;
    }

    // Source Address
    switch (hcCtl & kHcSrcAddrModeMask)
    {
    case kHcSrcAddrMode0:
        if ((hcCtl & kHcSrcAddrContext) == 0)
        {
            memcpy(&ip6Header.GetSource(), cur, sizeof(ip6Header.GetSource()));
            cur += sizeof(Ip6::Address);
        }

        break;

    case kHcSrcAddrMode1:
        ip6Header.GetSource().SetIid(cur);
        cur += Ip6::Address::kInterfaceIdentifierSize;
        break;

    case kHcSrcAddrMode2:
        ip6Header.GetSource().mFields.m8[11] = 0xff;
        ip6Header.GetSource().mFields.m8[12] = 0xfe;
        memcpy(ip6Header.GetSource().mFields.m8 + 14, cur, 2);
        cur += 2;
        break;

    case kHcSrcAddrMode3:
        ComputeIid(aMacSource, srcContext, ip6Header.GetSource());
        break;
    }

    if ((hcCtl & kHcSrcAddrContext) == 0)
    {
        if ((hcCtl & kHcSrcAddrModeMask) != 0)
        {
            ip6Header.GetSource().mFields.m16[0] = HostSwap16(0xfe80);
        }
    }
    else
    {
        VerifyOrExit(srcContextValid, error = kThreadError_Parse);
        CopyContext(srcContext, ip6Header.GetSource());
    }

    if ((hcCtl & kHcMulticast) == 0)
    {
        // Unicast Destination Address

        switch (hcCtl & kHcDstAddrModeMask)
        {
        case kHcDstAddrMode0:
            memcpy(&ip6Header.GetDestination(), cur, sizeof(ip6Header.GetDestination()));
            cur += sizeof(Ip6::Address);
            break;

        case kHcDstAddrMode1:
            ip6Header.GetDestination().SetIid(cur);
            cur += Ip6::Address::kInterfaceIdentifierSize;
            break;

        case kHcDstAddrMode2:
            ip6Header.GetDestination().mFields.m8[11] = 0xff;
            ip6Header.GetDestination().mFields.m8[12] = 0xfe;
            memcpy(ip6Header.GetDestination().mFields.m8 + 14, cur, 2);
            cur += 2;
            break;

        case kHcDstAddrMode3:
            ComputeIid(aMacDest, dstContext, ip6Header.GetDestination());
            break;
        }

        if ((hcCtl & kHcDstAddrContext) == 0)
        {
            if ((hcCtl & kHcDstAddrModeMask) != 0)
            {
                ip6Header.GetDestination().mFields.m16[0] = HostSwap16(0xfe80);
            }
        }
        else
        {
            VerifyOrExit(dstContextValid, error = kThreadError_Parse);
            CopyContext(dstContext, ip6Header.GetDestination());
        }
    }
    else
    {
        // Multicast Destination Address

        ip6Header.GetDestination().mFields.m8[0] = 0xff;

        if ((hcCtl & kHcDstAddrContext) == 0)
        {
            switch (hcCtl & kHcDstAddrModeMask)
            {
            case kHcDstAddrMode0:
                memcpy(ip6Header.GetDestination().mFields.m8, cur, sizeof(Ip6::Address));
                cur += sizeof(Ip6::Address);
                break;

            case kHcDstAddrMode1:
                ip6Header.GetDestination().mFields.m8[1] = cur[0];
                memcpy(ip6Header.GetDestination().mFields.m8 + 11, cur + 1, 5);
                cur += 6;
                break;

            case kHcDstAddrMode2:
                ip6Header.GetDestination().mFields.m8[1] = cur[0];
                memcpy(ip6Header.GetDestination().mFields.m8 + 13, cur + 1, 3);
                cur += 4;
                break;

            case kHcDstAddrMode3:
                ip6Header.GetDestination().mFields.m8[1] = 0x02;
                ip6Header.GetDestination().mFields.m8[15] = cur[0];
                cur++;
                break;
            }
        }
        else
        {
            switch (hcCtl & kHcDstAddrModeMask)
            {
            case 0:
                VerifyOrExit(dstContextValid, error = kThreadError_Parse);
                ip6Header.GetDestination().mFields.m8[1] = cur[0];
                ip6Header.GetDestination().mFields.m8[2] = cur[1];
                ip6Header.GetDestination().mFields.m8[3] = dstContext.mPrefixLength;
                memcpy(ip6Header.GetDestination().mFields.m8 + 4, dstContext.mPrefix, 8);
                memcpy(ip6Header.GetDestination().mFields.m8 + 12, cur + 2, 4);
                cur += 6;
                break;

            default:
                ExitNow(error = kThreadError_Parse);
            }
        }
    }

    if ((hcCtl & kHcNextHeader) != 0)
    {
        SuccessOrExit(error = DispatchToNextHeader(cur[0], nextHeader));
        ip6Header.SetNextHeader(nextHeader);
    }

exit:
    return (error == kThreadError_None) ? static_cast<int>(cur - aBuf) : -1;
}

int Lowpan::DecompressExtensionHeader(Message &aMessage, const uint8_t *aBuf, uint16_t aBufLength)
{
    const uint8_t *cur = aBuf;
    uint8_t hdr[2];
    uint8_t len;
    Ip6::IpProto nextHeader;
    int rval = -1;
    uint8_t ctl = cur[0];

    cur++;

    // next header
    if (ctl & kExtHdrNextHeader)
    {
        len = cur[0];
        cur++;

        SuccessOrExit(DispatchToNextHeader(cur[len], nextHeader));
        hdr[0] = static_cast<uint8_t>(nextHeader);
    }
    else
    {
        hdr[0] = cur[0];
        cur++;

        len = cur[0];
        cur++;
    }

    // length
    hdr[1] = BitVectorBytes(sizeof(hdr) + len) - 1;

    SuccessOrExit(aMessage.Append(hdr, sizeof(hdr)));
    aMessage.MoveOffset(sizeof(hdr));

    // payload
    SuccessOrExit(aMessage.Append(cur, len));
    aMessage.MoveOffset(len);
    cur += len;

    rval = static_cast<int>(cur - aBuf);

exit:
    (void)aBufLength;
    return rval;
}

int Lowpan::DecompressUdpHeader(Message &aMessage, const uint8_t *aBuf, uint16_t aBufLength, uint16_t aDatagramLength)
{
    ThreadError error = kThreadError_None;
    const uint8_t *cur = aBuf;
    Ip6::UdpHeader udpHeader;
    uint8_t udpCtl = cur[0];

    cur++;

    memset(&udpHeader, 0, sizeof(udpHeader));

    // source and dest ports
    switch (udpCtl & kUdpPortMask)
    {
    case 0:
        udpHeader.SetSourcePort(static_cast<uint16_t>((cur[0] << 8) | cur[1]));
        udpHeader.SetDestinationPort(static_cast<uint16_t>((cur[2] << 8) | cur[3]));
        cur += 4;
        break;

    case 1:
        udpHeader.SetSourcePort(static_cast<uint16_t>((cur[0] << 8) | cur[1]));
        udpHeader.SetDestinationPort(0xf000 | cur[2]);
        cur += 3;
        break;

    case 2:
        udpHeader.SetSourcePort(0xf000 | cur[0]);
        udpHeader.SetDestinationPort(static_cast<uint16_t>((cur[1] << 8) | cur[2]));
        cur += 3;
        break;

    case 3:
        udpHeader.SetSourcePort(0xf0b0 | (cur[0] >> 4));
        udpHeader.SetDestinationPort(0xf0b0 | (cur[0] & 0xf));
        cur += 1;
        break;
    }

    // checksum
    if ((udpCtl & kUdpChecksum) != 0)
    {
        ExitNow(error = kThreadError_Parse);
    }
    else
    {
        udpHeader.SetChecksum(static_cast<uint16_t>((cur[0] << 8) | cur[1]));
        cur += 2;
    }

    // length
    if (aDatagramLength == 0)
    {
        udpHeader.SetLength(sizeof(udpHeader) + static_cast<uint16_t>(aBufLength - (cur - aBuf)));
    }
    else
    {
        udpHeader.SetLength(aDatagramLength - aMessage.GetOffset());
    }

    aMessage.Append(&udpHeader, sizeof(udpHeader));
    aMessage.MoveOffset(sizeof(udpHeader));

exit:

    if (error != kThreadError_None)
    {
        return -1;
    }

    return static_cast<int>(cur - aBuf);
}

int Lowpan::Decompress(Message &aMessage, const Mac::Address &aMacSource, const Mac::Address &aMacDest,
                       const uint8_t *aBuf, uint16_t aBufLen, uint16_t aDatagramLength)
{
    ThreadError error = kThreadError_None;
    Ip6::Header ip6Header;
    const uint8_t *cur = aBuf;
    bool compressed;
    uint16_t remaining;
    int rval;

    compressed = (((static_cast<uint16_t>(cur[0]) << 8) | cur[1]) & kHcNextHeader) != 0;

    VerifyOrExit((rval = DecompressBaseHeader(ip6Header, aMacSource, aMacDest, aBuf)) >= 0, ;);
    cur += rval;

    SuccessOrExit(error = aMessage.Append(&ip6Header, sizeof(ip6Header)));
    SuccessOrExit(error = aMessage.SetOffset(sizeof(ip6Header)));

    while (compressed)
    {
        remaining = aBufLen - static_cast<uint16_t>(cur - aBuf);

        if ((cur[0] & kExtHdrDispatchMask) == kExtHdrDispatch)
        {
            compressed = (cur[0] & kExtHdrNextHeader) != 0;
            VerifyOrExit((rval = DecompressExtensionHeader(aMessage, cur, remaining)) >= 0,
                         error = kThreadError_Parse);
        }
        else if ((cur[0] & kUdpDispatchMask) == kUdpDispatch)
        {
            compressed = false;
            VerifyOrExit((rval = DecompressUdpHeader(aMessage, cur, remaining, aDatagramLength)) >= 0,
                         error = kThreadError_Parse);
        }
        else
        {
            ExitNow(error = kThreadError_Parse);
        }

        cur += rval;
    }

exit:

    if (error != kThreadError_None)
    {
        return -1;
    }

    return static_cast<int>(cur - aBuf);
}

}  // namespace Lowpan
}  // namespace Thread