Insert MPL Option in host-generated realm-local mcast messages. (#874)

src/core/common/message.cpp

@@ -315,12 +315,34 @@ ThreadError Message::Prepend(const void *aBuf, uint16_t aLength)
     mInfo.mLength += aLength;
     SetOffset(GetOffset() + aLength);
 
-    Write(0, aLength, aBuf);
+    if (aBuf != NULL)
+    {
+        Write(0, aLength, aBuf);
+    }
 
 exit:
     return error;
 }
 
+ThreadError Message::RemoveHeader(uint16_t aLength)
+{
+    assert(aLength <= mInfo.mLength);
+
+    mInfo.mReserved += aLength;
+    mInfo.mLength -= aLength;
+
+    if (mInfo.mOffset > aLength)
+    {
+        mInfo.mOffset -= aLength;
+    }
+    else
+    {
+        mInfo.mOffset = 0;
+    }
+
+    return kThreadError_None;
+}
+
 uint16_t Message::Read(uint16_t aOffset, uint16_t aLength, void *aBuf) const
 {
     Buffer *curBuffer;

src/core/common/message.hpp

@@ -326,6 +326,16 @@ public:
      */
     ThreadError Prepend(const void *aBuf, uint16_t aLength);
 
+    /**
+     * This method removes header bytes from the message.
+     *
+     * @param[in]  aLength  Number of header bytes to remove.
+     *
+     * @retval kThreadError_None  Successfully removed header bytes from the message.
+     *
+     */
+    ThreadError RemoveHeader(uint16_t aLength);
+
     /**
      * This method appends bytes to the end of the message.
      *

src/core/net/ip6.cpp

@@ -151,6 +151,170 @@ exit:
     return error;
 }
 
+ThreadError Ip6::InsertMplOption(Message &aMessage, Header &aIp6Header)
+{
+    ThreadError error = kThreadError_None;
+
+    VerifyOrExit(aIp6Header.GetDestination().IsRealmLocalMulticast(),);
+
+    aMessage.RemoveHeader(sizeof(aIp6Header));
+
+    if (aIp6Header.GetNextHeader() == kProtoHopOpts)
+    {
+        HopByHopHeader hbh;
+        uint8_t hbhLength = 0;
+        OptionMpl mplOption;
+
+        // read existing hop-by-hop option header
+        aMessage.Read(0, sizeof(hbh), &hbh);
+        hbhLength = (hbh.GetLength() + 1) * 8;
+
+        // increase existing hop-by-hop option header length by 8 bytes
+        hbh.SetLength(hbh.GetLength() + 1);
+        aMessage.Write(0, sizeof(hbh), &hbh);
+
+        // make space for MPL Option + padding by shifting hop-by-hop option header
+        aMessage.Prepend(NULL, 8);
+        aMessage.CopyTo(8, 0, hbhLength, aMessage);
+
+        // insert MPL Option
+        mMpl.InitOption(mplOption, aIp6Header.GetSource());
+        aMessage.Write(hbhLength, mplOption.GetTotalLength(), &mplOption);
+
+        // insert Pad Option (if needed)
+        if (mplOption.GetTotalLength() % 8)
+        {
+            OptionPadN padOption;
+            padOption.Init(8 - (mplOption.GetTotalLength() % 8));
+            aMessage.Write(hbhLength + mplOption.GetTotalLength(), padOption.GetTotalLength(), &padOption);
+        }
+
+        // increase IPv6 Payload Length
+        aIp6Header.SetPayloadLength(aIp6Header.GetPayloadLength() + 8);
+    }
+    else
+    {
+        SuccessOrExit(error = AddMplOption(aMessage, aIp6Header, aIp6Header.GetNextHeader(),
+                                           aIp6Header.GetPayloadLength()));
+    }
+
+    SuccessOrExit(error = aMessage.Prepend(&aIp6Header, sizeof(aIp6Header)));
+
+exit:
+    return error;
+}
+
+ThreadError Ip6::RemoveMplOption(Message &aMessage)
+{
+    ThreadError error = kThreadError_None;
+    Header ip6Header;
+    HopByHopHeader hbh;
+    uint16_t offset;
+    uint16_t endOffset;
+    uint16_t mplOffset = 0;
+    uint8_t mplLength = 0;
+    bool remove = false;
+
+    offset = 0;
+    aMessage.Read(offset, sizeof(ip6Header), &ip6Header);
+    offset += sizeof(ip6Header);
+    VerifyOrExit(ip6Header.GetNextHeader() == kProtoHopOpts,);
+
+    aMessage.Read(offset, sizeof(hbh), &hbh);
+    endOffset = offset + (hbh.GetLength() + 1) * 8;
+    VerifyOrExit(aMessage.GetLength() >= endOffset,);
+
+    offset += sizeof(hbh);
+
+    while (offset < endOffset)
+    {
+        OptionHeader option;
+
+        aMessage.Read(offset, sizeof(option), &option);
+
+        switch (option.GetType())
+        {
+        case OptionMpl::kType:
+            mplOffset = offset;
+            mplLength = option.GetLength();
+
+            if (mplOffset == sizeof(ip6Header) + sizeof(hbh) && hbh.GetLength() == 0)
+            {
+                // first and only IPv6 Option, remove IPv6 HBH Option header
+                remove = true;
+            }
+            else if (mplOffset + 8 == endOffset)
+            {
+                // last IPv6 Option, remove last 8 bytes
+                remove = true;
+            }
+
+            offset += sizeof(option) + option.GetLength();
+            break;
+
+        case OptionPad1::kType:
+            offset += sizeof(OptionPad1);
+            break;
+
+        case OptionPadN::kType:
+            offset += sizeof(option) + option.GetLength();
+            break;
+
+        default:
+            // encountered another option, now just replace MPL Option with PadN
+            remove = false;
+            offset += sizeof(option) + option.GetLength();
+            break;
+        }
+    }
+
+    // verify that IPv6 Options header is properly formed
+    VerifyOrExit(offset == endOffset,);
+
+    if (remove)
+    {
+        // last IPv6 Option, shrink HBH Option header
+        uint8_t buf[8];
+
+        offset = endOffset - sizeof(buf);
+
+        while (offset >= sizeof(buf))
+        {
+            aMessage.Read(offset - sizeof(buf), sizeof(buf), buf);
+            aMessage.Write(offset, sizeof(buf), buf);
+            offset -= sizeof(buf);
+        }
+
+        aMessage.RemoveHeader(sizeof(buf));
+
+        if (mplOffset == sizeof(ip6Header) + sizeof(hbh))
+        {
+            // remove entire HBH header
+            ip6Header.SetNextHeader(hbh.GetNextHeader());
+        }
+        else
+        {
+            // update HBH header length
+            hbh.SetLength(hbh.GetLength() - 1);
+            aMessage.Write(sizeof(ip6Header), sizeof(hbh), &hbh);
+        }
+
+        ip6Header.SetPayloadLength(ip6Header.GetPayloadLength() - sizeof(buf));
+        aMessage.Write(0, sizeof(ip6Header), &ip6Header);
+    }
+    else if (mplOffset != 0)
+    {
+        // replace MPL Option with PadN Option
+        OptionPadN padOption;
+
+        padOption.Init(sizeof(OptionHeader) + mplLength);
+        aMessage.Write(mplOffset, padOption.GetTotalLength(), &padOption);
+    }
+
+exit:
+    return error;
+}
+
 void Ip6::EnqueueDatagram(Message &aMessage)
 {
     mSendQueue.Enqueue(aMessage);
@@ -419,6 +583,7 @@ ThreadError Ip6::ProcessReceiveCallback(const Message &aMessage, const MessageIn
 
     // make a copy of the datagram to pass to host
     VerifyOrExit((messageCopy = aMessage.Clone()) != NULL, error = kThreadError_NoBufs);
+    RemoveMplOption(*messageCopy);
     mReceiveIp6DatagramCallback(messageCopy, mReceiveIp6DatagramCallbackContext);
 
 exit:
@@ -475,6 +640,11 @@ ThreadError Ip6::HandleDatagram(Message &message, Netif *netif, int8_t interface
         if (netif == NULL)
         {
             forward = true;
+
+            if (fromLocalHost)
+            {
+                SuccessOrExit(error = InsertMplOption(message, header));
+            }
         }
     }
     else

src/core/net/ip6.hpp

@@ -347,6 +347,8 @@ private:
                                        bool receive);
     ThreadError HandleFragment(Message &message);
     ThreadError AddMplOption(Message &message, Header &header, IpProto nextHeader, uint16_t payloadLength);
+    ThreadError InsertMplOption(Message &message, Header &header);
+    ThreadError RemoveMplOption(Message &aMessage);
     ThreadError HandleOptions(Message &message, Header &header, bool &forward);
     ThreadError HandlePayload(Message &message, MessageInfo &messageInfo, uint8_t ipproto);
     ThreadError ForwardMessage(Message &message, MessageInfo &messageInfo, uint8_t ipproto);

tests/scripts/Makefile.am

@@ -266,8 +266,6 @@ TESTS                                                              = \
     $(NULL)
 
 XFAIL_NCP_TESTS                                                    = \
-    thread-cert/Cert_5_3_02_RealmLocal.py                            \
-    thread-cert/Cert_6_4_02_RealmLocal.py                            \
     thread-cert/Cert_8_1_01_Commissioning.py                         \
     thread-cert/Cert_8_1_02_Commissioning.py                         \
     thread-cert/Cert_8_2_01_JoinerRouter.py                          \