otError Frame::GetSrcAddr(Address &address) const
{
otError error = OT_ERROR_NONE;
uint8_t index = FindSrcAddrIndex();
uint16_t fcf = GetFrameControlField();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
switch (fcf & kFcfSrcAddrMask)
{
case kFcfSrcAddrShort:
address.SetShort(Encoding::LittleEndian::ReadUint16(GetPsdu() + index));
break;
case kFcfSrcAddrExt:
address.SetExtended(GetPsdu() + index, /* reverse */ true);
break;
default:
address.SetNone();
break;
}
exit:
return error;
}
void Frame::SetFramePending(bool aFramePending)
{
if (aFramePending)
{
GetPsdu()[0] |= kFcfFramePending;
}
else
{
GetPsdu()[0] &= ~kFcfFramePending;
}
}
void Frame::SetAckRequest(bool aAckRequest)
{
if (aAckRequest)
{
GetPsdu()[0] |= kFcfAckRequest;
}
else
{
GetPsdu()[0] &= ~kFcfAckRequest;
}
}
otError Frame::SetDstAddr(ShortAddress aShortAddress)
{
assert((GetFrameControlField() & kFcfDstAddrMask) == kFcfDstAddrShort);
Encoding::LittleEndian::WriteUint16(aShortAddress, GetPsdu() + FindDstAddrIndex());
return OT_ERROR_NONE;
}
otError Frame::SetDstPanId(PanId aPanId)
{
uint8_t index = FindDstPanIdIndex();
assert(index != kInvalidIndex);
Encoding::LittleEndian::WriteUint16(aPanId, GetPsdu() + index);
return OT_ERROR_NONE;
}
const uint8_t *Frame::GetPayload(void) const
{
uint8_t index = FindPayloadIndex();
const uint8_t *payload = GetPsdu() + index;
VerifyOrExit(index != kInvalidIndex, payload = NULL);
exit:
return payload;
}
otError Frame::SetSrcPanId(PanId aPanId)
{
otError error = OT_ERROR_NONE;
uint8_t index = FindSrcPanIdIndex();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
Encoding::LittleEndian::WriteUint16(aPanId, GetPsdu() + index);
exit:
return error;
}
otError Frame::GetSrcPanId(PanId &aPanId) const
{
otError error = OT_ERROR_NONE;
uint8_t index = FindSrcPanIdIndex();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
aPanId = Encoding::LittleEndian::ReadUint16(GetPsdu() + index);
exit:
return error;
}
otError Frame::SetSrcAddr(ShortAddress aShortAddress)
{
uint8_t index = FindSrcAddrIndex();
assert((GetFrameControlField() & kFcfSrcAddrMask) == kFcfSrcAddrShort);
assert(index != kInvalidIndex);
Encoding::LittleEndian::WriteUint16(aShortAddress, GetPsdu() + index);
return OT_ERROR_NONE;
}
otError Frame::GetKeyIdMode(uint8_t &aKeyIdMode) const
{
otError error = OT_ERROR_NONE;
uint8_t index = FindSecurityHeaderIndex();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
aKeyIdMode = GetPsdu()[index] & kKeyIdModeMask;
exit:
return error;
}
otError Frame::SetCommandId(uint8_t aCommandId)
{
otError error = OT_ERROR_NONE;
uint8_t index = FindPayloadIndex();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
(GetPsdu() + index)[-1] = aCommandId;
exit:
return error;
}
const uint8_t *Frame::GetKeySource(void) const
{
uint8_t index = FindSecurityHeaderIndex();
const uint8_t *buf = GetPsdu() + index;
assert(index != kInvalidIndex);
// Security Control
buf += kSecurityControlSize + kFrameCounterSize;
return buf;
}
otError Frame::GetSecurityLevel(uint8_t &aSecurityLevel) const
{
otError error = OT_ERROR_NONE;
uint8_t index = FindSecurityHeaderIndex();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
aSecurityLevel = GetPsdu()[index] & kSecLevelMask;
exit:
return error;
}
otError Frame::SetFrameCounter(uint32_t aFrameCounter)
{
uint8_t index = FindSecurityHeaderIndex();
assert(index != kInvalidIndex);
// Security Control
index += kSecurityControlSize;
Encoding::LittleEndian::WriteUint32(aFrameCounter, GetPsdu() + index);
return OT_ERROR_NONE;
}
void Frame::SetKeySource(const uint8_t *aKeySource)
{
uint8_t keySourceLength;
uint8_t index = FindSecurityHeaderIndex();
uint8_t *buf = GetPsdu() + index;
assert(index != kInvalidIndex);
keySourceLength = GetKeySourceLength(buf[0] & kKeyIdModeMask);
buf += kSecurityControlSize + kFrameCounterSize;
memcpy(buf, aKeySource, keySourceLength);
}
otError Frame::GetFrameCounter(uint32_t &aFrameCounter) const
{
otError error = OT_ERROR_NONE;
uint8_t index = FindSecurityHeaderIndex();
VerifyOrExit(index != kInvalidIndex, error = OT_ERROR_PARSE);
// Security Control
index += kSecurityControlSize;
aFrameCounter = Encoding::LittleEndian::ReadUint32(GetPsdu() + index);
exit:
return error;
}
otError Frame::SetSrcAddr(const ExtAddress &aExtAddress)
{
uint8_t index = FindSrcAddrIndex();
uint8_t *buf = GetPsdu() + index;
assert((GetFrameControlField() & kFcfSrcAddrMask) == kFcfSrcAddrExt);
assert(index != kInvalidIndex);
for (unsigned int i = 0; i < sizeof(aExtAddress); i++)
{
buf[i] = aExtAddress.m8[sizeof(aExtAddress) - 1 - i];
}
return OT_ERROR_NONE;
}
otError Frame::SetKeyId(uint8_t aKeyId)
{
uint8_t keySourceLength;
uint8_t index = FindSecurityHeaderIndex();
uint8_t *buf = GetPsdu() + index;
assert(index != kInvalidIndex);
keySourceLength = GetKeySourceLength(buf[0] & kKeyIdModeMask);
buf += kSecurityControlSize + kFrameCounterSize + keySourceLength;
buf[0] = aKeyId;
return OT_ERROR_NONE;
}
otError Frame::GetKeyId(uint8_t &aKeyId) const
{
otError error = OT_ERROR_NONE;
uint8_t keySourceLength;
uint8_t index = FindSecurityHeaderIndex();
const uint8_t *buf = GetPsdu() + index;
VerifyOrExit(index != kInvalidIndex);
keySourceLength = GetKeySourceLength(buf[0] & kKeyIdModeMask);
buf += kSecurityControlSize + kFrameCounterSize + keySourceLength;
aKeyId = buf[0];
exit:
return error;
}
uint8_t Frame::GetFooterLength(void) const
{
uint8_t footerLength = 0;
uint8_t index = FindSecurityHeaderIndex();
VerifyOrExit(index != kInvalidIndex);
switch ((GetPsdu() + index)[0] & kSecLevelMask)
{
case kSecNone:
case kSecEnc:
footerLength += kMic0Size;
break;
case kSecMic32:
case kSecEncMic32:
footerLength += kMic32Size;
break;
case kSecMic64:
case kSecEncMic64:
footerLength += kMic64Size;
break;
case kSecMic128:
case kSecEncMic128:
footerLength += kMic128Size;
break;
}
exit:
// Frame Check Sequence
footerLength += kFcsSize;
return footerLength;
}
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;
}
uint8_t Frame::GetHeaderLength(void) const
{
return static_cast<uint8_t>(GetPayload() - GetPsdu());
}
uint16_t Frame::GetFrameControlField(void) const
{
return Encoding::LittleEndian::ReadUint16(GetPsdu());
}
const uint8_t *Frame::GetFooter(void) const
{
return GetPsdu() + GetPsduLength() - GetFooterLength();
}
ThreadError Frame::ValidatePsdu(void)
{
ThreadError error = kThreadError_Parse;
uint8_t offset = 0;
uint16_t fcf;
uint8_t footerLength = kFcsSize;
VerifyOrExit((offset += kFcfSize + kDsnSize) <= GetPsduLength(),);
fcf = static_cast<uint16_t>((GetPsdu()[1] << 8) | GetPsdu()[0]);
// Destinatinon PAN + Address
switch (fcf & Frame::kFcfDstAddrMask)
{
case Frame::kFcfDstAddrNone:
break;
case Frame::kFcfDstAddrShort:
offset += sizeof(PanId) + sizeof(ShortAddress);
break;
case Frame::kFcfDstAddrExt:
offset += sizeof(PanId) + sizeof(ExtAddress);
break;
default:
goto exit;
}
// Source PAN + Address
switch (fcf & Frame::kFcfSrcAddrMask)
{
case Frame::kFcfSrcAddrNone:
break;
case Frame::kFcfSrcAddrShort:
if ((fcf & Frame::kFcfPanidCompression) == 0)
{
offset += sizeof(PanId);
}
offset += sizeof(ShortAddress);
break;
case Frame::kFcfSrcAddrExt:
if ((fcf & Frame::kFcfPanidCompression) == 0)
{
offset += sizeof(PanId);
}
offset += sizeof(ExtAddress);
break;
default:
goto exit;
}
// Security Header
if (fcf & Frame::kFcfSecurityEnabled)
{
VerifyOrExit(offset <= GetPsduLength(),);
uint8_t secControl = GetPsdu()[offset];
switch (secControl & kKeyIdModeMask)
{
case kKeyIdMode0:
offset += kKeySourceSizeMode0;
break;
case kKeyIdMode1:
offset += kKeySourceSizeMode1 + kKeyIndexSize;
break;
case kKeyIdMode2:
offset += kKeySourceSizeMode2 + kKeyIndexSize;
break;
case kKeyIdMode3:
offset += kKeySourceSizeMode3 + kKeyIndexSize;
break;
}
switch (secControl & kSecLevelMask)
{
case kSecNone:
case kSecEnc:
footerLength += kMic0Size;
break;
case kSecMic32:
case kSecEncMic32:
footerLength += kMic32Size;
break;
case kSecMic64:
case kSecEncMic64:
footerLength += kMic64Size;
break;
case kSecMic128:
case kSecEncMic128:
footerLength += kMic128Size;
break;
}
}
otError Frame::InitMacHeader(uint16_t aFcf, uint8_t aSecurityControl)
{
uint8_t *bytes = GetPsdu();
uint8_t length = 0;
// Frame Control Field
Encoding::LittleEndian::WriteUint16(aFcf, bytes);
length += kFcfSize;
// Sequence Number
length += kDsnSize;
// Destination PAN + Address
switch (aFcf & kFcfDstAddrMask)
{
case kFcfDstAddrNone:
break;
case kFcfDstAddrShort:
length += sizeof(PanId) + sizeof(ShortAddress);
break;
case kFcfDstAddrExt:
length += sizeof(PanId) + sizeof(ExtAddress);
break;
default:
assert(false);
}
// Source PAN + Address
switch (aFcf & kFcfSrcAddrMask)
{
case kFcfSrcAddrNone:
break;
case kFcfSrcAddrShort:
if ((aFcf & kFcfPanidCompression) == 0)
{
length += sizeof(PanId);
}
length += sizeof(ShortAddress);
break;
case kFcfSrcAddrExt:
if ((aFcf & kFcfPanidCompression) == 0)
{
length += sizeof(PanId);
}
length += sizeof(ExtAddress);
break;
default:
assert(false);
}
// Security Header
if (aFcf & kFcfSecurityEnabled)
{
bytes[length] = aSecurityControl;
if (aSecurityControl & kSecLevelMask)
{
length += kSecurityControlSize + kFrameCounterSize;
}
switch (aSecurityControl & kKeyIdModeMask)
{
case kKeyIdMode0:
length += kKeySourceSizeMode0;
break;
case kKeyIdMode1:
length += kKeySourceSizeMode1 + kKeyIndexSize;
break;
case kKeyIdMode2:
length += kKeySourceSizeMode2 + kKeyIndexSize;
break;
case kKeyIdMode3:
length += kKeySourceSizeMode3 + kKeyIndexSize;
break;
}
}
// Command ID
if ((aFcf & kFcfFrameTypeMask) == kFcfFrameMacCmd)
{
length += kCommandIdSize;
}
SetPsduLength(length + GetFooterLength());
return OT_ERROR_NONE;
}
