otError Dataset::ApplyConfiguration(Instance &aInstance) const
{
ThreadNetif &netif = aInstance.GetThreadNetif();
Mac::Mac & mac = netif.GetMac();
otError error = OT_ERROR_NONE;
const Tlv * cur = reinterpret_cast<const Tlv *>(mTlvs);
const Tlv * end = reinterpret_cast<const Tlv *>(mTlvs + mLength);
VerifyOrExit(IsValid(), error = OT_ERROR_PARSE);
while (cur < end)
{
switch (cur->GetType())
{
case Tlv::kChannel:
{
uint8_t channel = static_cast<uint8_t>(static_cast<const ChannelTlv *>(cur)->GetChannel());
error = mac.SetPanChannel(channel);
if (error != OT_ERROR_NONE)
{
otLogWarnMeshCoP(aInstance, "DatasetManager::ApplyConfiguration() Failed to set channel to %d (%s)",
channel, otThreadErrorToString(error));
ExitNow();
}
break;
}
case Tlv::kChannelMask:
{
const ChannelMask0Entry *mask0Entry = static_cast<const ChannelMaskTlv *>(cur)->GetMask0Entry();
if (mask0Entry != NULL)
{
mac.SetSupportedChannelMask(mask0Entry->GetMask());
}
break;
}
case Tlv::kPanId:
mac.SetPanId(static_cast<const PanIdTlv *>(cur)->GetPanId());
break;
case Tlv::kExtendedPanId:
mac.SetExtendedPanId(static_cast<const ExtendedPanIdTlv *>(cur)->GetExtendedPanId());
break;
case Tlv::kNetworkName:
{
const NetworkNameTlv *name = static_cast<const NetworkNameTlv *>(cur);
mac.SetNetworkName(name->GetNetworkName(), name->GetLength());
break;
}
case Tlv::kNetworkMasterKey:
{
const NetworkMasterKeyTlv *key = static_cast<const NetworkMasterKeyTlv *>(cur);
netif.GetKeyManager().SetMasterKey(key->GetNetworkMasterKey());
break;
}
#if OPENTHREAD_FTD
case Tlv::kPSKc:
{
const PSKcTlv *pskc = static_cast<const PSKcTlv *>(cur);
netif.GetKeyManager().SetPSKc(pskc->GetPSKc());
break;
}
#endif
case Tlv::kMeshLocalPrefix:
{
const MeshLocalPrefixTlv *prefix = static_cast<const MeshLocalPrefixTlv *>(cur);
netif.GetMle().SetMeshLocalPrefix(prefix->GetMeshLocalPrefix());
break;
}
case Tlv::kSecurityPolicy:
{
const SecurityPolicyTlv *securityPolicy = static_cast<const SecurityPolicyTlv *>(cur);
netif.GetKeyManager().SetKeyRotation(securityPolicy->GetRotationTime());
netif.GetKeyManager().SetSecurityPolicyFlags(securityPolicy->GetFlags());
break;
}
default:
{
break;
}
}
cur = cur->GetNext();
}
exit:
return error;
}
otError Header::AppendOption(const Option &aOption)
{
otError error = OT_ERROR_NONE;
uint8_t *buf = mHeader.mBytes + mHeaderLength;
uint8_t *cur = buf + 1;
uint16_t optionDelta = aOption.mNumber - mOptionLast;
uint16_t optionLength;
// Assure that no option is inserted out of order.
VerifyOrExit(aOption.mNumber >= mOptionLast, error = OT_ERROR_INVALID_ARGS);
// Calculate the total option size and check the buffers.
optionLength = 1 + aOption.mLength;
optionLength += optionDelta < kOption1ByteExtensionOffset ? 0 :
(optionDelta < kOption2ByteExtensionOffset ? 1 : 2);
optionLength += aOption.mLength < kOption1ByteExtensionOffset ? 0 :
(aOption.mLength < kOption2ByteExtensionOffset ? 1 : 2);
VerifyOrExit(mHeaderLength + optionLength < kMaxHeaderLength, error = OT_ERROR_NO_BUFS);
// Insert option delta.
if (optionDelta < kOption1ByteExtensionOffset)
{
*buf = (optionDelta << Option::kOptionDeltaOffset) & Option::kOptionDeltaMask;
}
else if (optionDelta < kOption2ByteExtensionOffset)
{
*buf |= kOption1ByteExtension << Option::kOptionDeltaOffset;
*cur++ = (optionDelta - kOption1ByteExtensionOffset) & 0xff;
}
else
{
*buf |= kOption2ByteExtension << Option::kOptionDeltaOffset;
optionDelta -= kOption2ByteExtensionOffset;
*cur++ = optionDelta >> 8;
*cur++ = optionDelta & 0xff;
}
// Insert option length.
if (aOption.mLength < kOption1ByteExtensionOffset)
{
*buf |= aOption.mLength;
}
else if (aOption.mLength < kOption2ByteExtensionOffset)
{
*buf |= kOption1ByteExtension;
*cur++ = (aOption.mLength - kOption1ByteExtensionOffset) & 0xff;
}
else
{
*buf |= kOption2ByteExtension;
optionLength = aOption.mLength - kOption2ByteExtensionOffset;
*cur++ = optionLength >> 8;
*cur++ = optionLength & 0xff;
}
// Insert option value.
memcpy(cur, aOption.mValue, aOption.mLength);
cur += aOption.mLength;
mHeaderLength += static_cast<uint8_t>(cur - buf);
mOptionLast = aOption.mNumber;
exit:
return error;
}
ThreadError Mac::ProcessReceiveSecurity(const Address &aSrcAddr, Neighbor *aNeighbor)
{
ThreadError error = kThreadError_None;
uint8_t securityLevel;
uint32_t frameCounter;
uint8_t nonce[kNonceSize];
uint8_t tag[Frame::kMaxMicSize];
uint8_t tagLength;
uint8_t keyid;
uint32_t keySequence;
const uint8_t *macKey;
Crypto::AesCcm aesCcm;
mReceiveFrame.SetSecurityValid(false);
if (mReceiveFrame.GetSecurityEnabled() == false)
{
ExitNow();
}
VerifyOrExit(aNeighbor != NULL, error = kThreadError_Security);
mReceiveFrame.GetSecurityLevel(securityLevel);
mReceiveFrame.GetFrameCounter(frameCounter);
GenerateNonce(aSrcAddr.mExtAddress, frameCounter, securityLevel, nonce);
tagLength = mReceiveFrame.GetFooterLength() - Frame::kFcsSize;
mReceiveFrame.GetKeyId(keyid);
keyid--;
if (keyid == (mKeyManager.GetCurrentKeySequence() & 0x7f))
{
// same key index
keySequence = mKeyManager.GetCurrentKeySequence();
macKey = mKeyManager.GetCurrentMacKey();
VerifyOrExit(aNeighbor->mPreviousKey == true || frameCounter >= aNeighbor->mValid.mLinkFrameCounter,
error = kThreadError_Security);
}
else if (aNeighbor->mPreviousKey &&
mKeyManager.IsPreviousKeyValid() &&
keyid == (mKeyManager.GetPreviousKeySequence() & 0x7f))
{
// previous key index
keySequence = mKeyManager.GetPreviousKeySequence();
macKey = mKeyManager.GetPreviousMacKey();
VerifyOrExit(frameCounter >= aNeighbor->mValid.mLinkFrameCounter, error = kThreadError_Security);
}
else if (keyid == ((mKeyManager.GetCurrentKeySequence() + 1) & 0x7f))
{
// next key index
keySequence = mKeyManager.GetCurrentKeySequence() + 1;
macKey = mKeyManager.GetTemporaryMacKey(keySequence);
}
else
{
for (Receiver *receiver = mReceiveHead; receiver; receiver = receiver->mNext)
{
receiver->HandleReceivedFrame(mReceiveFrame, kThreadError_Security);
}
ExitNow(error = kThreadError_Security);
}
aesCcm.SetKey(macKey, 16);
aesCcm.Init(mReceiveFrame.GetHeaderLength(), mReceiveFrame.GetPayloadLength(),
tagLength, nonce, sizeof(nonce));
aesCcm.Header(mReceiveFrame.GetHeader(), mReceiveFrame.GetHeaderLength());
aesCcm.Payload(mReceiveFrame.GetPayload(), mReceiveFrame.GetPayload(), mReceiveFrame.GetPayloadLength(),
false);
aesCcm.Finalize(tag, &tagLength);
VerifyOrExit(memcmp(tag, mReceiveFrame.GetFooter(), tagLength) == 0, error = kThreadError_Security);
if (keySequence > mKeyManager.GetCurrentKeySequence())
{
mKeyManager.SetCurrentKeySequence(keySequence);
}
if (keySequence == mKeyManager.GetCurrentKeySequence())
{
aNeighbor->mPreviousKey = false;
}
aNeighbor->mValid.mLinkFrameCounter = frameCounter + 1;
mReceiveFrame.SetSecurityValid(true);
exit:
return error;
}
otError Dataset::Set(const otOperationalDataset &aDataset)
{
otError error = OT_ERROR_NONE;
MeshCoP::ActiveTimestampTlv activeTimestampTlv;
VerifyOrExit(aDataset.mComponents.mIsActiveTimestampPresent, error = OT_ERROR_INVALID_ARGS);
activeTimestampTlv.Init();
activeTimestampTlv.SetSeconds(aDataset.mActiveTimestamp);
activeTimestampTlv.SetTicks(0);
Set(activeTimestampTlv);
if (mType == Tlv::kPendingTimestamp)
{
MeshCoP::PendingTimestampTlv pendingTimestampTlv;
VerifyOrExit(aDataset.mComponents.mIsPendingTimestampPresent, error = OT_ERROR_INVALID_ARGS);
pendingTimestampTlv.Init();
pendingTimestampTlv.SetSeconds(aDataset.mPendingTimestamp);
pendingTimestampTlv.SetTicks(0);
Set(pendingTimestampTlv);
if (aDataset.mComponents.mIsDelayPresent)
{
MeshCoP::DelayTimerTlv tlv;
tlv.Init();
tlv.SetDelayTimer(aDataset.mDelay);
Set(tlv);
}
}
if (aDataset.mComponents.mIsChannelPresent)
{
MeshCoP::ChannelTlv tlv;
tlv.Init();
tlv.SetChannelPage(0);
tlv.SetChannel(aDataset.mChannel);
Set(tlv);
}
if (aDataset.mComponents.mIsChannelMaskPage0Present)
{
MeshCoP::ChannelMask0Tlv tlv;
tlv.Init();
tlv.SetMask(aDataset.mChannelMaskPage0);
Set(tlv);
}
if (aDataset.mComponents.mIsExtendedPanIdPresent)
{
MeshCoP::ExtendedPanIdTlv tlv;
tlv.Init();
tlv.SetExtendedPanId(aDataset.mExtendedPanId);
Set(tlv);
}
if (aDataset.mComponents.mIsMeshLocalPrefixPresent)
{
MeshCoP::MeshLocalPrefixTlv tlv;
tlv.Init();
tlv.SetMeshLocalPrefix(aDataset.mMeshLocalPrefix.m8);
Set(tlv);
}
if (aDataset.mComponents.mIsMasterKeyPresent)
{
MeshCoP::NetworkMasterKeyTlv tlv;
tlv.Init();
tlv.SetNetworkMasterKey(aDataset.mMasterKey);
Set(tlv);
}
if (aDataset.mComponents.mIsNetworkNamePresent)
{
MeshCoP::NetworkNameTlv tlv;
tlv.Init();
tlv.SetNetworkName(aDataset.mNetworkName.m8);
Set(tlv);
}
if (aDataset.mComponents.mIsPanIdPresent)
{
MeshCoP::PanIdTlv tlv;
tlv.Init();
tlv.SetPanId(aDataset.mPanId);
Set(tlv);
}
if (aDataset.mComponents.mIsPSKcPresent)
{
MeshCoP::PSKcTlv tlv;
tlv.Init();
tlv.SetPSKc(aDataset.mPSKc.m8);
Set(tlv);
}
if (aDataset.mComponents.mIsSecurityPolicyPresent)
{
MeshCoP::SecurityPolicyTlv tlv;
tlv.Init();
tlv.SetRotationTime(aDataset.mSecurityPolicy.mRotationTime);
tlv.SetFlags(aDataset.mSecurityPolicy.mFlags);
Set(tlv);
}
mUpdateTime = TimerMilli::GetNow();
exit:
return error;
}
static inline bool isDataRequest(const uint8_t *frame)
{
const uint8_t *cur = frame;
uint8_t securityControl;
bool rval;
// FCF + DSN
cur += 2 + 1;
VerifyOrExit(isFrameTypeMacCmd(frame), rval = false);
// Destination PAN + Address
switch (frame[1] & IEEE802154_DST_ADDR_MASK)
{
case IEEE802154_DST_ADDR_SHORT:
cur += sizeof(otPanId) + sizeof(otShortAddress);
break;
case IEEE802154_DST_ADDR_EXT:
cur += sizeof(otPanId) + sizeof(otExtAddress);
break;
default:
ExitNow(rval = false);
}
// Source PAN + Address
switch (frame[1] & IEEE802154_SRC_ADDR_MASK)
{
case IEEE802154_SRC_ADDR_SHORT:
if (!isPanIdCompressed(frame))
{
cur += sizeof(otPanId);
}
cur += sizeof(otShortAddress);
break;
case IEEE802154_SRC_ADDR_EXT:
if (!isPanIdCompressed(frame))
{
cur += sizeof(otPanId);
}
cur += sizeof(otExtAddress);
break;
default:
ExitNow(rval = false);
}
// Security Control + Frame Counter + Key Identifier
if (isSecurityEnabled(frame))
{
securityControl = *cur;
if (securityControl & IEEE802154_SEC_LEVEL_MASK)
{
cur += 1 + 4;
}
switch (securityControl & IEEE802154_KEY_ID_MODE_MASK)
{
case IEEE802154_KEY_ID_MODE_0:
cur += 0;
break;
case IEEE802154_KEY_ID_MODE_1:
cur += 1;
break;
case IEEE802154_KEY_ID_MODE_2:
cur += 5;
break;
case IEEE802154_KEY_ID_MODE_3:
cur += 9;
break;
}
}
// Command ID
rval = cur[0] == IEEE802154_MACCMD_DATA_REQ;
exit:
return rval;
}
otError EnergyScanClient::SendQuery(uint32_t aChannelMask, uint8_t aCount, uint16_t aPeriod,
uint16_t aScanDuration, const Ip6::Address &aAddress,
otCommissionerEnergyReportCallback aCallback, void *aContext)
{
ThreadNetif &netif = GetNetif();
otError error = OT_ERROR_NONE;
Coap::Header header;
MeshCoP::CommissionerSessionIdTlv sessionId;
MeshCoP::ChannelMask0Tlv channelMask;
MeshCoP::CountTlv count;
MeshCoP::PeriodTlv period;
MeshCoP::ScanDurationTlv scanDuration;
Ip6::MessageInfo messageInfo;
Message *message = NULL;
VerifyOrExit(netif.GetCommissioner().IsActive(), error = OT_ERROR_INVALID_STATE);
header.Init(aAddress.IsMulticast() ? OT_COAP_TYPE_NON_CONFIRMABLE : OT_COAP_TYPE_CONFIRMABLE,
OT_COAP_CODE_POST);
header.SetToken(Coap::Header::kDefaultTokenLength);
header.AppendUriPathOptions(OT_URI_PATH_ENERGY_SCAN);
header.SetPayloadMarker();
VerifyOrExit((message = MeshCoP::NewMeshCoPMessage(netif.GetCoap(), header)) != NULL,
error = OT_ERROR_NO_BUFS);
sessionId.Init();
sessionId.SetCommissionerSessionId(netif.GetCommissioner().GetSessionId());
SuccessOrExit(error = message->Append(&sessionId, sizeof(sessionId)));
channelMask.Init();
channelMask.SetMask(aChannelMask);
SuccessOrExit(error = message->Append(&channelMask, sizeof(channelMask)));
count.Init();
count.SetCount(aCount);
SuccessOrExit(error = message->Append(&count, sizeof(count)));
period.Init();
period.SetPeriod(aPeriod);
SuccessOrExit(error = message->Append(&period, sizeof(period)));
scanDuration.Init();
scanDuration.SetScanDuration(aScanDuration);
SuccessOrExit(error = message->Append(&scanDuration, sizeof(scanDuration)));
messageInfo.SetSockAddr(netif.GetMle().GetMeshLocal16());
messageInfo.SetPeerAddr(aAddress);
messageInfo.SetPeerPort(kCoapUdpPort);
messageInfo.SetInterfaceId(netif.GetInterfaceId());
SuccessOrExit(error = netif.GetCoap().SendMessage(*message, messageInfo));
otLogInfoMeshCoP(GetInstance(), "sent energy scan query");
mCallback = aCallback;
mContext = aContext;
exit:
if (error != OT_ERROR_NONE && message != NULL)
{
message->Free();
}
return error;
}
otError Message::AppendOption(uint16_t aNumber, uint16_t aLength, const void *aValue)
{
otError error = OT_ERROR_NONE;
uint16_t optionDelta = aNumber - GetHelpData().mOptionLast;
uint16_t optionLength;
uint8_t buf[kMaxOptionHeaderSize] = {0};
uint8_t *cur = &buf[1];
// Assure that no option is inserted out of order.
VerifyOrExit(aNumber >= GetHelpData().mOptionLast, error = OT_ERROR_INVALID_ARGS);
// Calculate the total option size and check the buffers.
optionLength = 1 + aLength;
optionLength += optionDelta < kOption1ByteExtensionOffset ? 0 : (optionDelta < kOption2ByteExtensionOffset ? 1 : 2);
optionLength += aLength < kOption1ByteExtensionOffset ? 0 : (aLength < kOption2ByteExtensionOffset ? 1 : 2);
VerifyOrExit(GetLength() + optionLength < kMaxHeaderLength, error = OT_ERROR_NO_BUFS);
// Insert option delta.
if (optionDelta < kOption1ByteExtensionOffset)
{
*buf = (optionDelta << kOptionDeltaOffset) & kOptionDeltaMask;
}
else if (optionDelta < kOption2ByteExtensionOffset)
{
*buf |= kOption1ByteExtension << kOptionDeltaOffset;
*cur++ = (optionDelta - kOption1ByteExtensionOffset) & 0xff;
}
else
{
*buf |= kOption2ByteExtension << kOptionDeltaOffset;
optionDelta -= kOption2ByteExtensionOffset;
*cur++ = optionDelta >> 8;
*cur++ = optionDelta & 0xff;
}
// Insert option length.
if (aLength < kOption1ByteExtensionOffset)
{
*buf |= aLength;
}
else if (aLength < kOption2ByteExtensionOffset)
{
*buf |= kOption1ByteExtension;
*cur++ = (aLength - kOption1ByteExtensionOffset) & 0xff;
}
else
{
*buf |= kOption2ByteExtension;
optionLength = aLength - kOption2ByteExtensionOffset;
*cur++ = optionLength >> 8;
*cur++ = optionLength & 0xff;
}
Append(buf, static_cast<uint16_t>(cur - buf));
Append(aValue, aLength);
GetHelpData().mOptionLast = aNumber;
GetHelpData().mHeaderLength = GetLength();
exit:
return error;
}
const otCoapOption *Message::GetNextOption(void)
{
otError error = OT_ERROR_NONE;
uint16_t optionDelta;
uint16_t optionLength;
uint8_t buf[kMaxOptionHeaderSize];
uint8_t * cur = buf + 1;
otCoapOption *rval = NULL;
VerifyOrExit(GetHelpData().mNextOptionOffset < GetLength(), error = OT_ERROR_NOT_FOUND);
Read(GetHelpData().mNextOptionOffset, sizeof(buf), buf);
optionDelta = buf[0] >> 4;
optionLength = buf[0] & 0xf;
GetHelpData().mNextOptionOffset += sizeof(uint8_t);
if (optionDelta < kOption1ByteExtension)
{
// do nothing
}
else if (optionDelta == kOption1ByteExtension)
{
optionDelta = kOption1ByteExtensionOffset + cur[0];
GetHelpData().mNextOptionOffset += sizeof(uint8_t);
cur++;
}
else if (optionDelta == kOption2ByteExtension)
{
optionDelta = kOption2ByteExtensionOffset + static_cast<uint16_t>((cur[0] << 8) | cur[1]);
GetHelpData().mNextOptionOffset += sizeof(uint16_t);
cur += 2;
}
else
{
VerifyOrExit(optionLength == 0xf, error = OT_ERROR_PARSE);
ExitNow(error = OT_ERROR_NOT_FOUND);
}
if (optionLength < kOption1ByteExtension)
{
// do nothing
}
else if (optionLength == kOption1ByteExtension)
{
optionLength = kOption1ByteExtensionOffset + cur[0];
GetHelpData().mNextOptionOffset += sizeof(uint8_t);
}
else if (optionLength == kOption2ByteExtension)
{
optionLength = kOption2ByteExtensionOffset + static_cast<uint16_t>((cur[0] << 8) | cur[1]);
GetHelpData().mNextOptionOffset += sizeof(uint16_t);
}
else
{
ExitNow();
}
rval = &GetHelpData().mOption;
rval->mNumber += optionDelta;
rval->mLength = optionLength;
GetHelpData().mNextOptionOffset += optionLength;
exit:
if (error == OT_ERROR_PARSE)
{
GetHelpData().mNextOptionOffset = 0;
}
return rval;
}
uint8_t Frame::FindPayloadIndex(void) const
{
uint8_t index = 0;
uint16_t fcf;
// Frame Control
index += kFcfSize;
// Sequence Number
index += kDsnSize;
VerifyOrExit((index + kFcsSize) <= GetPsduLength(), index = kInvalidIndex);
fcf = GetFrameControlField();
// Destination PAN + Address
switch (fcf & kFcfDstAddrMask)
{
case kFcfDstAddrNone:
break;
case kFcfDstAddrShort:
index += sizeof(PanId) + sizeof(ShortAddress);
break;
case kFcfDstAddrExt:
index += sizeof(PanId) + sizeof(ExtAddress);
break;
default:
ExitNow(index = kInvalidIndex);
}
// Source PAN + Address
switch (fcf & kFcfSrcAddrMask)
{
case kFcfSrcAddrNone:
break;
case kFcfSrcAddrShort:
if ((fcf & kFcfPanidCompression) == 0)
{
index += sizeof(PanId);
}
index += sizeof(ShortAddress);
break;
case kFcfSrcAddrExt:
if ((fcf & kFcfPanidCompression) == 0)
{
index += sizeof(PanId);
}
index += sizeof(ExtAddress);
break;
default:
ExitNow(index = kInvalidIndex);
}
VerifyOrExit((index + kFcsSize) <= GetPsduLength(), index = kInvalidIndex);
// Security Control + Frame Counter + Key Identifier
if ((fcf & kFcfSecurityEnabled) != 0)
{
uint8_t securityControl = *(GetPsdu() + index);
index += kSecurityControlSize + kFrameCounterSize;
switch (securityControl & kKeyIdModeMask)
{
case kKeyIdMode0:
index += kKeySourceSizeMode0;
break;
case kKeyIdMode1:
index += kKeySourceSizeMode1 + kKeyIndexSize;
break;
case kKeyIdMode2:
index += kKeySourceSizeMode2 + kKeyIndexSize;
break;
case kKeyIdMode3:
index += kKeySourceSizeMode3 + kKeyIndexSize;
break;
}
}
// Command ID
if ((fcf & kFcfFrameTypeMask) == kFcfFrameMacCmd)
{
index += kCommandIdSize;
}
exit:
return index;
}
