diff --git a/src/core/thread/lowpan.cpp b/src/core/thread/lowpan.cpp index f96119fff..980ae412f 100644 --- a/src/core/thread/lowpan.cpp +++ b/src/core/thread/lowpan.cpp @@ -45,6 +45,7 @@ using ot::Encoding::BigEndian::HostSwap16; using ot::Encoding::BigEndian::ReadUint16; +using ot::Encoding::BigEndian::WriteUint16; namespace ot { namespace Lowpan { @@ -1206,58 +1207,116 @@ exit: return (error == OT_ERROR_NONE) ? static_cast(compressedLength) : -1; } -otError MeshHeader::Init(const uint8_t *aFrame, uint16_t aFrameLength) +//--------------------------------------------------------------------------------------------------------------------- +// MeshHeader + +void MeshHeader::Init(uint16_t aSource, uint16_t aDestination, uint8_t aHopsLeft) { - otError error = OT_ERROR_NONE; + mSource = aSource; + mDestination = aDestination; + mHopsLeft = aHopsLeft; +} - VerifyOrExit(aFrameLength >= 1, error = OT_ERROR_PARSE); - mDispatchHopsLeft = *aFrame++; - aFrameLength--; +bool MeshHeader::IsMeshHeader(const uint8_t *aFrame, uint16_t aFrameLength) +{ + return (aFrameLength >= kMinHeaderLength) && ((*aFrame & kDispatchMask) == kDispatch); +} - if (IsDeepHopsLeftField()) +otError MeshHeader::ParseFrom(const uint8_t *aFrame, uint16_t aFrameLength, uint16_t &aHeaderLength) +{ + otError error = OT_ERROR_PARSE; + uint8_t dispatch; + + VerifyOrExit(aFrameLength >= kMinHeaderLength); + dispatch = *aFrame++; + + VerifyOrExit((dispatch & (kDispatchMask | kSourceShort | kDestShort)) == (kDispatch | kSourceShort | kDestShort)); + + mHopsLeft = (dispatch & kHopsLeftMask); + + if (mHopsLeft == kDeepHopsLeft) { - VerifyOrExit(aFrameLength >= 1, error = OT_ERROR_PARSE); - mDeepHopsLeft = *aFrame++; - aFrameLength--; + VerifyOrExit(aFrameLength >= kDeepHopsHeaderLength); + mHopsLeft = *aFrame++; + aHeaderLength = kDeepHopsHeaderLength; } else { - mDeepHopsLeft = 0; + aHeaderLength = kMinHeaderLength; } - VerifyOrExit(aFrameLength >= sizeof(mAddress), error = OT_ERROR_PARSE); - memcpy(&mAddress, aFrame, sizeof(mAddress)); + mSource = ReadUint16(aFrame); + mDestination = ReadUint16(aFrame + 2); + + error = OT_ERROR_NONE; exit: return error; } -otError MeshHeader::Init(const Message &aMessage) +otError MeshHeader::ParseFrom(const Message &aMessage) { - otError error = OT_ERROR_NONE; - uint16_t offset = 0; - uint16_t bytesRead; + uint16_t headerLength; - bytesRead = aMessage.Read(offset, sizeof(mDispatchHopsLeft), &mDispatchHopsLeft); - VerifyOrExit(bytesRead == sizeof(mDispatchHopsLeft), error = OT_ERROR_PARSE); - offset += bytesRead; + return ParseFrom(aMessage, headerLength); +} - if (IsDeepHopsLeftField()) +otError MeshHeader::ParseFrom(const Message &aMessage, uint16_t &aHeaderLength) +{ + uint8_t frame[kDeepHopsHeaderLength]; + uint16_t frameLength; + + frameLength = aMessage.Read(/* aOffset */ 0, sizeof(frame), frame); + + return ParseFrom(frame, frameLength, aHeaderLength); +} + +uint16_t MeshHeader::GetHeaderLength(void) const +{ + return (mHopsLeft >= kDeepHopsLeft) ? kDeepHopsHeaderLength : kMinHeaderLength; +} + +void MeshHeader::DecrementHopsLeft(void) +{ + if (mHopsLeft > 0) { - bytesRead = aMessage.Read(offset, sizeof(mDeepHopsLeft), &mDeepHopsLeft); - VerifyOrExit(bytesRead == sizeof(mDeepHopsLeft), error = OT_ERROR_PARSE); - offset += bytesRead; + mHopsLeft--; + } +} + +uint16_t MeshHeader::WriteTo(uint8_t *aFrame) const +{ + uint8_t *cur = aFrame; + uint8_t dispatch = (kDispatch | kSourceShort | kDestShort); + + if (mHopsLeft < kDeepHopsLeft) + { + *cur++ = (dispatch | mHopsLeft); } else { - mDeepHopsLeft = 0; + *cur++ = (dispatch | kDeepHopsLeft); + *cur++ = mHopsLeft; } - bytesRead = aMessage.Read(offset, sizeof(mAddress), &mAddress); - VerifyOrExit(bytesRead == sizeof(mAddress), error = OT_ERROR_PARSE); + WriteUint16(mSource, cur); + cur += sizeof(uint16_t); -exit: - return error; + WriteUint16(mDestination, cur); + cur += sizeof(uint16_t); + + return static_cast(cur - aFrame); +} + +uint16_t MeshHeader::WriteTo(Message &aMessage, uint16_t aOffset) const +{ + uint8_t frame[kDeepHopsHeaderLength]; + uint16_t headerLength; + + headerLength = WriteTo(frame); + aMessage.Write(aOffset, headerLength, frame); + + return headerLength; } otError FragmentHeader::Init(const uint8_t *aFrame, uint16_t aFrameLength) diff --git a/src/core/thread/lowpan.hpp b/src/core/thread/lowpan.hpp index 3017c8079..28aef2a99 100644 --- a/src/core/thread/lowpan.hpp +++ b/src/core/thread/lowpan.hpp @@ -389,7 +389,6 @@ private: * This class implements Mesh Header generation and processing. * */ -OT_TOOL_PACKED_BEGIN class MeshHeader { public: @@ -399,74 +398,79 @@ public: }; /** - * Default constructor for the object. + * This method initializes the Mesh Header with a given Mesh Source, Mesh Destination and Hops Left value. + * + * @param[in] aSource The Mesh Source address. + * @param[in] aDestination The Mesh Destination address. + * @param[in] aHopsLeft The Hops Left value. * */ - MeshHeader(void) { memset(this, 0, sizeof(*this)); } + void Init(uint16_t aSource, uint16_t aDestination, uint8_t aHopsLeft); /** - * This method initializes the header. + * This static method indicates whether or not the header (in a given frame) is a Mesh Header. * - */ - void Init(void) { mDispatchHopsLeft = kDispatch | kSourceShort | kDestinationShort; } - - /** - * This method initializes the mesh header from a frame @p aFrame. - * - * @param[in] aFrame The pointer to the frame. - * @param[in] aFrameLength The length of the frame. - * - * @retval OT_ERROR_NONE Mesh Header initialized successfully. - * @retval OT_ERROR_PARSE Mesh Header could not be parsed from @p aFrame. - * - */ - otError Init(const uint8_t *aFrame, uint16_t aFrameLength); - - /** - * This method initializes the mesh header from a message object @p aMessage. - * - * @param[in] aMessage The message object. - * - * @retval OT_ERROR_NONE Mesh Header initialized successfully. - * @retval OT_ERROR_PARSE Mesh Header could not be parsed from @p aMessage. - * - */ - otError Init(const Message &aMessage); - - /** - * This method indicates whether or not the header is a Mesh Header. + * @note This method checks whether the first byte in header/frame (dispatch byte) matches the Mesh Header dispatch + * It does not fully parse and validate the Mesh Header. `ParseFrom()` method can be used to fully parse and + * validate the header. * * @retval TRUE If the header matches the Mesh Header dispatch value. * @retval FALSE If the header does not match the Mesh Header dispatch value. * */ - bool IsMeshHeader(void) const { return (mDispatchHopsLeft & kDispatchMask) == kDispatch; } + static bool IsMeshHeader(const uint8_t *aFrame, uint16_t aFrameLength); /** - * This method indicates whether or not the Mesh Header appears to be well-formed. + * This method parses the Mesh Header from a frame @p aFrame. * - * @retval TRUE If the header appears to be well-formed. - * @retval FALSE If the header does not appear to be well-formed. + * @param[in] aFrame The pointer to the frame. + * @param[in] aFrameLength The length of the frame. + * @param[out] aHeaderLength A reference to a variable to output the parsed header length (on success). + * + * @retval OT_ERROR_NONE Mesh Header parsed successfully. + * @retval OT_ERROR_PARSE Mesh Header could not be parsed. * */ - bool IsValid(void) const { return (mDispatchHopsLeft & kSourceShort) && (mDispatchHopsLeft & kDestinationShort); } + otError ParseFrom(const uint8_t *aFrame, uint16_t aFrameLength, uint16_t &aHeaderLength); /** - * This method indicates whether or not the header contains Deep Hops Left field. + * This method parses the Mesh Header from a given message. * - * @retval TRUE If the header does contain Deep Hops Left field. - * @retval FALSE If the header does not contain Deep Hops Left field. + * @note The Mesh Header is read from offset zero within the @p aMessage. + * + * @param[in] aMessage The message to read from. + * + * @retval OT_ERROR_NONE Mesh Header parsed successfully. + * @retval OT_ERROR_PARSE Mesh Header could not be parsed. * */ - bool IsDeepHopsLeftField(void) const { return (mDispatchHopsLeft & kHopsLeftMask) == kDeepHopsLeft; } + otError ParseFrom(const Message &aMessage); /** - * This static method returns the size of the Mesh Header in bytes. + * This method parses the Mesh Header from a given message. * - * @returns The size of the Mesh Header in bytes. + * @note The Mesh Header is read from offset zero within the @p aMessage. + * + * @param[in] aMessage The message to read from. + * @param[out] aHeaderLength A reference to a variable to output the parsed header length (on success). + * + * @retval OT_ERROR_NONE Mesh Header parsed successfully. + * @retval OT_ERROR_PARSE Mesh Header could not be parsed. * */ - uint8_t GetHeaderLength(void) const { return sizeof(*this) - (IsDeepHopsLeftField() ? 0 : sizeof(mDeepHopsLeft)); } + otError ParseFrom(const Message &aMessage, uint16_t &aHeaderLength); + + /** + * This method returns the the Mesh Header length when written to a frame. + * + * @note The returned value from this method gives the header length (number of bytes) when the header is written + * to a frame or message. This should not be used to determine the parsed length (number of bytes read) when the + * Mesh Header is parsed from a frame/message (using `ParseFrom()` methods). + * + * @returns The length of the Mesh Header (in bytes) when written to a frame. + * + */ + uint16_t GetHeaderLength(void) const; /** * This method returns the Hops Left value. @@ -474,29 +478,13 @@ public: * @returns The Hops Left value. * */ - uint8_t GetHopsLeft(void) const - { - return IsDeepHopsLeftField() ? mDeepHopsLeft : mDispatchHopsLeft & kHopsLeftMask; - } + uint8_t GetHopsLeft(void) const { return mHopsLeft; } /** - * This method sets the Hops Left value. - * - * @param[in] aHops The Hops Left value. + * This method decrements the Hops Left value (if it is not zero). * */ - void SetHopsLeft(uint8_t aHops) - { - if (aHops < kDeepHopsLeft && !IsDeepHopsLeftField()) - { - mDispatchHopsLeft = (mDispatchHopsLeft & ~kHopsLeftMask) | aHops; - } - else - { - mDispatchHopsLeft = (mDispatchHopsLeft & ~kHopsLeftMask) | kDeepHopsLeft; - mDeepHopsLeft = aHops; - } - } + void DecrementHopsLeft(void); /** * This method returns the Mesh Source address. @@ -504,15 +492,7 @@ public: * @returns The Mesh Source address. * */ - uint16_t GetSource(void) const { return HostSwap16(mAddress.mSource); } - - /** - * This method sets the Mesh Source address. - * - * @param[in] aSource The Mesh Source address. - * - */ - void SetSource(uint16_t aSource) { mAddress.mSource = HostSwap16(aSource); } + uint16_t GetSource(void) const { return mSource; } /** * This method returns the Mesh Destination address. @@ -520,53 +500,51 @@ public: * @returns The Mesh Destination address. * */ - uint16_t GetDestination(void) const { return HostSwap16(mAddress.mDestination); } + uint16_t GetDestination(void) const { return mDestination; } /** - * This method sets the Mesh Destination address. + * This method writes the Mesh Header into a given frame. * - * @param[in] aDestination The Mesh Destination address. + * @note This method expects the frame buffer to have enough space for the entire Mesh Header. + * + * @param[out] aFrame The pointer to the frame buffer to write to. + * + * @returns The header length (number of bytes written). * */ - void SetDestination(uint16_t aDestination) { mAddress.mDestination = HostSwap16(aDestination); } + uint16_t WriteTo(uint8_t *aFrame) const; /** - * This method appends Mesh Header to the @p aFrame frame. + * This method writes the Mesh Header to a message at a given offset. * - * @param[in] aFrame The pointer to the frame. + * @note This method expects the @p aMessage length to be already set such that there is enough space for the + * entire Mesh Header to be written. + * + * @param[out] aMessage A message to write the Mesh Header into. + * @param[in] aOffset The offset at which to write the header. + * + * @returns The header length (number of bytes written). * */ - void AppendTo(uint8_t *aFrame) const - { - *aFrame++ = mDispatchHopsLeft; - - if (IsDeepHopsLeftField()) - { - *aFrame++ = mDeepHopsLeft; - } - - memcpy(aFrame, &mAddress, sizeof(mAddress)); - } + uint16_t WriteTo(Message &aMessage, uint16_t aOffset) const; private: enum { - kDispatch = 2 << 6, - kDispatchMask = 3 << 6, - kHopsLeftMask = 0x0f, - kSourceShort = 1 << 5, - kDestinationShort = 1 << 4, - kDeepHopsLeft = 0x0f + kDispatch = 2 << 6, + kDispatchMask = 3 << 6, + kHopsLeftMask = 0x0f, + kSourceShort = 1 << 5, + kDestShort = 1 << 4, + kDeepHopsLeft = 0x0f, + kMinHeaderLength = sizeof(uint8_t) + sizeof(uint16_t) + sizeof(uint16_t), // dispatch byte + src + dest + kDeepHopsHeaderLength = kMinHeaderLength + sizeof(uint8_t), // min header + deep hops }; - uint8_t mDispatchHopsLeft; - uint8_t mDeepHopsLeft; - struct OT_TOOL_PACKED_FIELD - { - uint16_t mSource; - uint16_t mDestination; - } mAddress; -} OT_TOOL_PACKED_END; + uint16_t mSource; + uint16_t mDestination; + uint8_t mHopsLeft; +}; /** * This class implements Fragment Header generation and parsing. diff --git a/src/core/thread/mesh_forwarder.cpp b/src/core/thread/mesh_forwarder.cpp index aff629f42..41aa70d74 100644 --- a/src/core/thread/mesh_forwarder.cpp +++ b/src/core/thread/mesh_forwarder.cpp @@ -393,12 +393,12 @@ otError MeshForwarder::SkipMeshHeader(const uint8_t *&aFrame, uint16_t &aFrameLe { otError error = OT_ERROR_NONE; Lowpan::MeshHeader meshHeader; + uint16_t headerLength; - VerifyOrExit(aFrameLength >= 1 && reinterpret_cast(aFrame)->IsMeshHeader()); - - SuccessOrExit(error = meshHeader.Init(aFrame, aFrameLength)); - aFrame += meshHeader.GetHeaderLength(); - aFrameLength -= meshHeader.GetHeaderLength(); + VerifyOrExit(Lowpan::MeshHeader::IsMeshHeader(aFrame, aFrameLength)); + SuccessOrExit(error = meshHeader.ParseFrom(aFrame, aFrameLength, headerLength)); + aFrame += headerLength; + aFrameLength -= headerLength; exit: return error; @@ -674,6 +674,7 @@ start: { Mle::MleRouter & mle = Get(); Lowpan::MeshHeader meshHeader; + uint16_t meshHeaderLength; uint8_t hopsLeft; if (mle.GetRole() == OT_DEVICE_ROLE_CHILD) @@ -704,13 +705,10 @@ start: hopsLeft += 1; } - meshHeader.Init(); - meshHeader.SetHopsLeft(hopsLeft + Lowpan::MeshHeader::kAdditionalHopsLeft); - meshHeader.SetSource(aMeshSource); - meshHeader.SetDestination(aMeshDest); - meshHeader.AppendTo(payload); - payload += meshHeader.GetHeaderLength(); - headerLength += meshHeader.GetHeaderLength(); + meshHeader.Init(aMeshSource, aMeshDest, hopsLeft + Lowpan::MeshHeader::kAdditionalHopsLeft); + meshHeaderLength = meshHeader.WriteTo(payload); + payload += meshHeaderLength; + headerLength += meshHeaderLength; } #endif @@ -1061,8 +1059,7 @@ void MeshForwarder::HandleReceivedFrame(Mac::RxFrame &aFrame) switch (aFrame.GetType()) { case Mac::Frame::kFcfFrameData: - if (payloadLength >= sizeof(Lowpan::MeshHeader) && - reinterpret_cast(payload)->IsMeshHeader()) + if (Lowpan::MeshHeader::IsMeshHeader(payload, payloadLength)) { #if OPENTHREAD_FTD HandleMesh(payload, payloadLength, macSource, linkInfo); diff --git a/src/core/thread/mesh_forwarder_ftd.cpp b/src/core/thread/mesh_forwarder_ftd.cpp index 7e39c3753..5604b9675 100644 --- a/src/core/thread/mesh_forwarder_ftd.cpp +++ b/src/core/thread/mesh_forwarder_ftd.cpp @@ -253,7 +253,7 @@ void MeshForwarder::RemoveMessages(Child &aChild, uint8_t aSubType) { Lowpan::MeshHeader meshHeader; - IgnoreReturnValue(meshHeader.Init(*message)); + IgnoreReturnValue(meshHeader.ParseFrom(*message)); if (&aChild == static_cast(mle.GetNeighbor(meshHeader.GetDestination()))) { @@ -342,7 +342,7 @@ otError MeshForwarder::UpdateMeshRoute(Message &aMessage) Neighbor * neighbor; uint16_t nextHop; - IgnoreReturnValue(meshHeader.Init(aMessage)); + IgnoreReturnValue(meshHeader.ParseFrom(aMessage)); nextHop = Get().GetNextHop(meshHeader.GetDestination()); @@ -506,11 +506,12 @@ void MeshForwarder::HandleMesh(uint8_t * aFrame, Mac::Address meshDest; Mac::Address meshSource; Lowpan::MeshHeader meshHeader; - - SuccessOrExit(error = meshHeader.Init(aFrame, aFrameLength)); + uint16_t headerLength; // Security Check: only process Mesh Header frames that had security enabled. - VerifyOrExit(aLinkInfo.mLinkSecurity && meshHeader.IsValid(), error = OT_ERROR_SECURITY); + VerifyOrExit(aLinkInfo.mLinkSecurity, error = OT_ERROR_SECURITY); + + SuccessOrExit(error = meshHeader.ParseFrom(aFrame, aFrameLength, headerLength)); meshSource.SetShort(meshHeader.GetSource()); meshDest.SetShort(meshHeader.GetDestination()); @@ -520,8 +521,8 @@ void MeshForwarder::HandleMesh(uint8_t * aFrame, if (meshDest.GetShort() == Get().GetShortAddress() || Get().IsMinimalChild(meshDest.GetShort())) { - aFrame += meshHeader.GetHeaderLength(); - aFrameLength -= meshHeader.GetHeaderLength(); + aFrame += headerLength; + aFrameLength -= headerLength; if (reinterpret_cast(aFrame)->IsFragmentHeader()) { @@ -538,21 +539,25 @@ void MeshForwarder::HandleMesh(uint8_t * aFrame, } else if (meshHeader.GetHopsLeft() > 0) { - uint8_t priority = kDefaultMsgPriority; + uint8_t priority = kDefaultMsgPriority; + uint16_t offset = 0; Get().ResolveRoutingLoops(aMacSource.GetShort(), meshDest.GetShort()); SuccessOrExit(error = CheckReachability(aFrame, aFrameLength, meshSource, meshDest)); - meshHeader.SetHopsLeft(meshHeader.GetHopsLeft() - 1); - meshHeader.AppendTo(aFrame); + meshHeader.DecrementHopsLeft(); - GetForwardFramePriority(aFrame + meshHeader.GetHeaderLength(), aFrameLength - meshHeader.GetHeaderLength(), - meshSource, meshDest, priority); - VerifyOrExit((message = Get().New(Message::kType6lowpan, priority)) != NULL, - error = OT_ERROR_NO_BUFS); - SuccessOrExit(error = message->SetLength(aFrameLength)); - message->Write(0, aFrameLength, aFrame); + aFrame += headerLength; + aFrameLength -= headerLength; + + GetForwardFramePriority(aFrame, aFrameLength, meshSource, meshDest, priority); + message = Get().New(Message::kType6lowpan, priority); + VerifyOrExit(message != NULL, error = OT_ERROR_NO_BUFS); + + SuccessOrExit(error = message->SetLength(meshHeader.GetHeaderLength() + aFrameLength)); + offset += meshHeader.WriteTo(*message, offset); + message->Write(offset, aFrameLength, aFrame); message->SetLinkSecurityEnabled(aLinkInfo.mLinkSecurity); message->SetPanId(aLinkInfo.mPanId); message->AddRss(aLinkInfo.mRss); @@ -846,14 +851,14 @@ otError MeshForwarder::LogMeshFragmentHeader(MessageAction aAction, bool shouldLogRss; Lowpan::MeshHeader meshHeader; Lowpan::FragmentHeader fragmentHeader; + uint16_t meshHeaderLength; - SuccessOrExit(meshHeader.Init(aMessage)); - VerifyOrExit(meshHeader.IsMeshHeader()); + SuccessOrExit(meshHeader.ParseFrom(aMessage, meshHeaderLength)); aMeshSource.SetShort(meshHeader.GetSource()); aMeshDest.SetShort(meshHeader.GetDestination()); - aOffset = meshHeader.GetHeaderLength(); + aOffset = meshHeaderLength; if (fragmentHeader.Init(aMessage, aOffset) == OT_ERROR_NONE) {