42 lines
1.8 KiB
C++
42 lines
1.8 KiB
C++
#include "icsneo/communication/packet/supportedfeaturespacket.h"
|
|
#include "icsneo/communication/message/supportedfeaturesmessage.h"
|
|
|
|
using namespace icsneo;
|
|
|
|
static constexpr uint16_t SupportedFeaturesCommandVersion = 1;
|
|
static constexpr size_t NumSupportedFeaturesFields =
|
|
(static_cast<size_t>(SupportedFeature::numSupportedFeatures) + 31) / 32;
|
|
#pragma pack(push, 2)
|
|
struct SupportedFeaturesResponse {
|
|
ExtendedResponseMessage::ResponseHeader header;
|
|
uint16_t cmdVersion;
|
|
uint16_t numValidBits;
|
|
uint32_t featuresFields[NumSupportedFeaturesFields];
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
std::shared_ptr<SupportedFeaturesMessage> SupportedFeaturesPacket::DecodeToMessage(const std::vector<uint8_t>& bytes) {
|
|
auto msg = std::make_shared<SupportedFeaturesMessage>();
|
|
// Length check: At least a header, a 2-byte cmdVersion field, and a 2-byte numValidBits field.
|
|
if(bytes.size() < sizeof(ExtendedResponseMessage::ResponseHeader) + 4) {
|
|
return msg; // Empty
|
|
}
|
|
// Get a reference to the payload to fully validate the length
|
|
const auto& response = *reinterpret_cast<const SupportedFeaturesResponse*>(bytes.data());
|
|
// Expected size is the header, cmdVersion and numValidBits fields, plus the number of 32-bit bitfields in the response based on numValidBits
|
|
auto expectedSize = sizeof(ExtendedResponseMessage::ResponseHeader) + 4 + ((response.numValidBits + 31) / 32) * 4;
|
|
// If the response is malformed (too small), return an empty message
|
|
if(bytes.size() < expectedSize) {
|
|
return msg; // Empty
|
|
}
|
|
unsigned int loopLimit = std::min<unsigned int>(response.numValidBits, NumSupportedFeaturesFields);
|
|
for(unsigned int i = 0; i < loopLimit; ++i) {
|
|
uint32_t wordOffset = i / 32;
|
|
uint32_t bitOffset = i % 32;
|
|
if((response.featuresFields[wordOffset] >> bitOffset) & 1) {
|
|
msg->features.insert(static_cast<SupportedFeature>(i));
|
|
}
|
|
}
|
|
return msg;
|
|
}
|