From a2f5a1ec88ca10cb8d0990d1e0c3e0de75104248 Mon Sep 17 00:00:00 2001 From: DevMiner Date: Thu, 3 Aug 2023 14:42:59 +0200 Subject: [PATCH 1/3] refactor(network): use correct int*_t types for `send*` in NetworkConnection --- src/network/connection.cpp | 57 +++++++++++++++++++++----------------- src/network/connection.h | 6 ++-- 2 files changed, 35 insertions(+), 28 deletions(-) diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 1f60ad876..31a6939d2 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -89,7 +89,7 @@ bool Connection::endPacket() { MUST_TRANSFER_BOOL((innerPacketSize > 0)); m_IsBundle = false; - + if (m_BundlePacketInnerCount == 0) { sendPacketType(PACKET_BUNDLE); sendPacketNumber(); @@ -128,13 +128,13 @@ bool Connection::endBundle() { MUST_TRANSFER_BOOL(m_IsBundle); m_IsBundle = false; - + MUST_TRANSFER_BOOL((m_BundlePacketInnerCount > 0)); return endPacket(); } -size_t Connection::write(const uint8_t *buffer, size_t size) { +size_t Connection::write(const uint8_t* buffer, size_t size) { if (m_IsBundle) { if (m_BundlePacketPosition + size > sizeof(m_Packet)) { return 0; @@ -146,9 +146,7 @@ size_t Connection::write(const uint8_t *buffer, size_t size) { return m_UDP.write(buffer, size); } -size_t Connection::write(uint8_t byte) { - return write(&byte, 1); -} +size_t Connection::write(uint8_t byte) { return write(&byte, 1); } bool Connection::sendFloat(float f) { convert_to_chars(f, m_Buf); @@ -158,19 +156,19 @@ bool Connection::sendFloat(float f) { bool Connection::sendByte(uint8_t c) { return write(&c, 1) != 0; } -bool Connection::sendShort(uint16_t i) { +bool Connection::sendShort(int16_t i) { convert_to_chars(i, m_Buf); return write(m_Buf, sizeof(i)) != 0; } -bool Connection::sendInt(uint32_t i) { +bool Connection::sendInt(int32_t i) { convert_to_chars(i, m_Buf); return write(m_Buf, sizeof(i)) != 0; } -bool Connection::sendLong(uint64_t l) { +bool Connection::sendU64(uint64_t l) { convert_to_chars(l, m_Buf); return write(m_Buf, sizeof(l)) != 0; @@ -185,9 +183,9 @@ bool Connection::sendPacketNumber() { return true; } - uint64_t pn = m_PacketNumber++; + auto pn = m_PacketNumber++; - return sendLong(pn); + return sendU64(pn); } bool Connection::sendShortString(const char* str) { @@ -391,7 +389,7 @@ void Connection::sendTrackerDiscovery() { MUST(sendPacketType(PACKET_HANDSHAKE)); // Packet number is always 0 for handshake - MUST(sendLong(0)); + MUST(sendU64(0)); MUST(sendInt(BOARD)); // This is kept for backwards compatibility, // but the latest SlimeVR server will not initialize trackers @@ -511,7 +509,7 @@ void Connection::returnLastPacket(int len) { MUST(endPacket()); } -void Connection::updateSensorState(std::vector & sensors) { +void Connection::updateSensorState(std::vector& sensors) { if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) { return; } @@ -525,7 +523,7 @@ void Connection::updateSensorState(std::vector & sensors) { } } -void Connection::maybeRequestFeatureFlags() { +void Connection::maybeRequestFeatureFlags() { if (m_ServerFeatures.isAvailable() || m_FeatureFlagsRequestAttempts >= 15) { return; } @@ -557,6 +555,8 @@ void Connection::searchForServer() { m_UDP.remotePort() ); m_Logger.traceArray("UDP packet contents: ", m_Packet, len); +#else + (void)len; #endif // Handshake is different, it has 3 in the first byte, not the 4th, and data @@ -571,9 +571,9 @@ void Connection::searchForServer() { m_ServerPort = m_UDP.remotePort(); m_LastPacketTimestamp = millis(); m_Connected = true; - + m_FeatureFlagsRequestAttempts = 0; - m_ServerFeatures = ServerFeatures { }; + m_ServerFeatures = ServerFeatures{}; statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false); ledManager.off(); @@ -603,7 +603,11 @@ void Connection::searchForServer() { void Connection::reset() { m_Connected = false; - std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE); + std::fill( + m_AckedSensorState, + m_AckedSensorState + MAX_IMU_COUNT, + SensorStatus::SENSOR_OFFLINE + ); m_UDP.begin(m_ServerPort); @@ -611,7 +615,7 @@ void Connection::reset() { } void Connection::update() { - std::vector & sensors = sensorManager.getSensors(); + std::vector& sensors = sensorManager.getSensors(); updateSensorState(sensors); maybeRequestFeatureFlags(); @@ -625,7 +629,11 @@ void Connection::update() { statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true); m_Connected = false; - std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE); + std::fill( + m_AckedSensorState, + m_AckedSensorState + MAX_IMU_COUNT, + SensorStatus::SENSOR_OFFLINE + ); m_Logger.warn("Connection to server timed out"); return; @@ -697,16 +705,15 @@ void Connection::update() { } bool hadFlags = m_ServerFeatures.isAvailable(); - uint32_t flagsLength = len - 12; m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength); if (!hadFlags) { - #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED - if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) { - m_Logger.debug("Server supports packet bundling"); - } - #endif +#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED + if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) { + m_Logger.debug("Server supports packet bundling"); + } +#endif } break; diff --git a/src/network/connection.h b/src/network/connection.h index 1231ee247..670c974db 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -128,9 +128,9 @@ class Connection { bool sendPacketNumber(); bool sendFloat(float f); bool sendByte(uint8_t c); - bool sendShort(uint16_t i); - bool sendInt(uint32_t i); - bool sendLong(uint64_t l); + bool sendShort(int16_t i); + bool sendInt(int32_t i); + bool sendU64(uint64_t l); bool sendBytes(const uint8_t* c, size_t length); bool sendShortString(const char* str); bool sendLongString(const char* str); From 33fe59d3076f979c8cd6f382d636d76b3f67d848 Mon Sep 17 00:00:00 2001 From: DevMiner Date: Fri, 4 Aug 2023 17:49:51 +0200 Subject: [PATCH 2/3] refactor(network): clean up packet bundling --- src/network/connection.cpp | 132 ++++++++++++++++++++----------------- src/network/connection.h | 46 +++++++++---- 2 files changed, 103 insertions(+), 75 deletions(-) diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 31a6939d2..3195a143a 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -67,7 +67,11 @@ namespace Network { bool Connection::beginPacket() { if (m_IsBundle) { - m_BundlePacketPosition = 0; + m_BundleInnerStart = m_BundleInnerPosition; + // We have to advance two bytes, since the first two bytes are reserved for the + // packet length + m_BundleInnerPosition += 2; + return true; } @@ -84,21 +88,14 @@ bool Connection::beginPacket() { bool Connection::endPacket() { if (m_IsBundle) { - uint32_t innerPacketSize = m_BundlePacketPosition; - - MUST_TRANSFER_BOOL((innerPacketSize > 0)); + auto innerPacketSize = getBundleInnerSize(); - m_IsBundle = false; + // We have to go back to the start of the packet and write the size + convert_to_chars((uint16_t)innerPacketSize, m_Buf); + memcpy(m_BundleInnerStart, m_Buf, 2); - if (m_BundlePacketInnerCount == 0) { - sendPacketType(PACKET_BUNDLE); - sendPacketNumber(); - } - sendShort(innerPacketSize); - sendBytes(m_Packet, innerPacketSize); + m_BundlePacketCount++; - m_BundlePacketInnerCount++; - m_IsBundle = true; return true; } @@ -115,39 +112,67 @@ bool Connection::endPacket() { bool Connection::beginBundle() { MUST_TRANSFER_BOOL(m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)); + + memset(m_Bundle, 0, sizeof(m_Bundle)); + m_BundleInnerPosition = m_Bundle; + m_BundlePacketCount = 0; + MUST_TRANSFER_BOOL(m_Connected); MUST_TRANSFER_BOOL(!m_IsBundle); - MUST_TRANSFER_BOOL(beginPacket()); m_IsBundle = true; - m_BundlePacketInnerCount = 0; + return true; } -bool Connection::endBundle() { - MUST_TRANSFER_BOOL(m_IsBundle); +bool Connection::sendBundle() { + MUST_TRANSFER_BOOL(m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)); + MUST_TRANSFER_BOOL(!m_IsBundle); + MUST_TRANSFER_BOOL((m_BundlePacketCount > 0)); + + MUST_TRANSFER_BOOL(beginPacket()); + + MUST_TRANSFER_BOOL(sendPacketType(PACKET_BUNDLE)); + MUST_TRANSFER_BOOL(sendPacketNumber()); + MUST_TRANSFER_BOOL(write(m_Bundle, getBundleSize())); + + MUST_TRANSFER_BOOL(endPacket()); + + return true; +} +bool Connection::endBundle() { m_IsBundle = false; - MUST_TRANSFER_BOOL((m_BundlePacketInnerCount > 0)); + auto ret = sendBundle(); + + memset(m_Buf, 0, sizeof(m_Buf)); + m_BundleInnerStart = m_Buf; + m_BundleInnerPosition = m_Buf; + m_BundlePacketCount = 0; - return endPacket(); + return ret; } size_t Connection::write(const uint8_t* buffer, size_t size) { if (m_IsBundle) { - if (m_BundlePacketPosition + size > sizeof(m_Packet)) { + if (getBundleSize() + size > MAX_BUNDLE_SIZE) { + m_Logger.error("Bundled packet too large"); + + // TODO: Drop the currently forming packet + return 0; } - memcpy(m_Packet + m_BundlePacketPosition, buffer, size); - m_BundlePacketPosition += size; + + memcpy(m_BundleInnerPosition, buffer, size); + m_BundleInnerPosition += size; + return size; } + return m_UDP.write(buffer, size); } -size_t Connection::write(uint8_t byte) { return write(&byte, 1); } - bool Connection::sendFloat(float f) { convert_to_chars(f, m_Buf); @@ -156,13 +181,13 @@ bool Connection::sendFloat(float f) { bool Connection::sendByte(uint8_t c) { return write(&c, 1) != 0; } -bool Connection::sendShort(int16_t i) { +bool Connection::sendU16(uint16_t i) { convert_to_chars(i, m_Buf); return write(m_Buf, sizeof(i)) != 0; } -bool Connection::sendInt(int32_t i) { +bool Connection::sendI32(int32_t i) { convert_to_chars(i, m_Buf); return write(m_Buf, sizeof(i)) != 0; @@ -197,18 +222,12 @@ bool Connection::sendShortString(const char* str) { return true; } -bool Connection::sendPacketType(uint8_t type) { - MUST_TRANSFER_BOOL(sendByte(0)); - MUST_TRANSFER_BOOL(sendByte(0)); - MUST_TRANSFER_BOOL(sendByte(0)); - - return sendByte(type); -} +bool Connection::sendPacketType(int32_t type) { return sendI32(type); } bool Connection::sendLongString(const char* str) { int size = strlen(str); - MUST_TRANSFER_BOOL(sendInt(size)); + MUST_TRANSFER_BOOL(sendI32(size)); return sendBytes((const uint8_t*)str, size); } @@ -390,16 +409,16 @@ void Connection::sendTrackerDiscovery() { MUST(sendPacketType(PACKET_HANDSHAKE)); // Packet number is always 0 for handshake MUST(sendU64(0)); - MUST(sendInt(BOARD)); + MUST(sendI32(BOARD)); // This is kept for backwards compatibility, // but the latest SlimeVR server will not initialize trackers // with firmware build > 8 until it recieves a sensor info packet - MUST(sendInt(IMU)); - MUST(sendInt(HARDWARE_MCU)); - MUST(sendInt(0)); - MUST(sendInt(0)); - MUST(sendInt(0)); - MUST(sendInt(FIRMWARE_BUILD_NUMBER)); + MUST(sendI32(IMU)); + MUST(sendI32(HARDWARE_MCU)); + MUST(sendI32(0)); + MUST(sendI32(0)); + MUST(sendI32(0)); + MUST(sendI32(FIRMWARE_BUILD_NUMBER)); MUST(sendShortString(FIRMWARE_VERSION)); // MAC address string MUST(sendBytes(mac, 6)); @@ -435,19 +454,19 @@ void Connection::sendInspectionRawIMUData( MUST(sendByte(sensorId)); MUST(sendByte(PACKET_INSPECTION_DATATYPE_INT)); - MUST(sendInt(rX)); - MUST(sendInt(rY)); - MUST(sendInt(rZ)); + MUST(sendI32(rX)); + MUST(sendI32(rY)); + MUST(sendI32(rZ)); MUST(sendByte(rA)); - MUST(sendInt(aX)); - MUST(sendInt(aY)); - MUST(sendInt(aZ)); + MUST(sendI32(aX)); + MUST(sendI32(aY)); + MUST(sendI32(aZ)); MUST(sendByte(aA)); - MUST(sendInt(mX)); - MUST(sendInt(mY)); - MUST(sendInt(mZ)); + MUST(sendI32(mX)); + MUST(sendI32(mY)); + MUST(sendI32(mZ)); MUST(sendByte(mA)); MUST(endPacket()); @@ -618,13 +637,14 @@ void Connection::update() { std::vector& sensors = sensorManager.getSensors(); updateSensorState(sensors); - maybeRequestFeatureFlags(); if (!m_Connected) { searchForServer(); return; } + maybeRequestFeatureFlags(); + if (m_LastPacketTimestamp + TIMEOUT < millis()) { statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true); @@ -647,17 +667,7 @@ void Connection::update() { m_LastPacketTimestamp = millis(); int len = m_UDP.read(m_Packet, sizeof(m_Packet)); -#ifdef DEBUG_NETWORK - m_Logger.trace( - "Received %d bytes from %s, port %d", - packetSize, - m_UDP.remoteIP().toString().c_str(), - m_UDP.remotePort() - ); - m_Logger.traceArray("UDP packet contents: ", m_Packet, len); -#else (void)packetSize; -#endif switch (convert_chars(m_Packet)) { case PACKET_RECEIVE_HEARTBEAT: diff --git a/src/network/connection.h b/src/network/connection.h index 670c974db..0088c12b8 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -26,11 +26,11 @@ #include #include +#include "featureflags.h" #include "globals.h" #include "quat.h" #include "sensors/sensor.h" #include "wifihandler.h" -#include "featureflags.h" namespace SlimeVR { namespace Network { @@ -107,29 +107,26 @@ class Connection { ); #endif - const ServerFeatures& getServerFeatureFlags() { - return m_ServerFeatures; - } + const ServerFeatures& getServerFeatureFlags() { return m_ServerFeatures; } bool beginBundle(); bool endBundle(); private: - void updateSensorState(std::vector & sensors); + void updateSensorState(std::vector& sensors); void maybeRequestFeatureFlags(); bool beginPacket(); bool endPacket(); - size_t write(const uint8_t *buffer, size_t size); - size_t write(uint8_t byte); + size_t write(const uint8_t* buffer, size_t size); - bool sendPacketType(uint8_t type); + bool sendPacketType(int32_t type); bool sendPacketNumber(); bool sendFloat(float f); bool sendByte(uint8_t c); - bool sendShort(int16_t i); - bool sendInt(int32_t i); + bool sendU16(uint16_t c); + bool sendI32(int32_t i); bool sendU64(uint64_t l); bool sendBytes(const uint8_t* c, size_t length); bool sendShortString(const char* str); @@ -152,7 +149,11 @@ class Connection { SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); WiFiUDP m_UDP; - unsigned char m_Packet[128]; // buffer for incoming packets + /* + The current incoming packet that is being handled + TODO: remove this from the class and make it a local variable + */ + uint8_t m_Packet[128]; uint64_t m_PacketNumber = 0; int m_ServerPort = 6969; @@ -168,10 +169,27 @@ class Connection { ServerFeatures m_ServerFeatures{}; bool m_IsBundle = false; - uint16_t m_BundlePacketPosition = 0; - uint16_t m_BundlePacketInnerCount = 0; + /* `53` is the maximum size of any packet that could be bundled, which is the + * inspection packet. The additional `2` bytes are from the field which describes + * how long the next bundled packet is. If you're having bundle size issues, then + * you forgot to increase MAX_IMU_COUNT in `defines.h`. */ + constexpr static size_t MAX_BUNDLE_SIZE = MAX_IMU_COUNT * (53 + 2); + /* The bundle that is currently being written. */ + uint8_t m_Bundle[MAX_BUNDLE_SIZE]; + /* Points into `m_Bundle` to indicate where we currently are. */ + uint8_t* m_BundleInnerPosition = m_Bundle; + /* Points to where we started writing the current packet into `m_Bundle`. */ + uint8_t* m_BundleInnerStart = m_Bundle; + /* Count of packets that are in the currently forming bundle. */ + uint16_t m_BundlePacketCount = 0; + + const size_t getBundleSize() const { return m_BundleInnerPosition - m_Bundle; } + const size_t getBundleInnerSize() const { + return m_BundleInnerPosition - m_BundleInnerStart - 2; + } + bool sendBundle(); - unsigned char m_Buf[8]; + uint8_t m_Buf[8]; }; } // namespace Network From 78d4a1de5dbe09d9c5911e3978a5e097ae64dc5f Mon Sep 17 00:00:00 2001 From: DevMiner Date: Mon, 2 Oct 2023 00:40:53 +0200 Subject: [PATCH 3/3] refactor: feature flags --- src/network/connection.cpp | 17 ++-- src/network/connection.h | 8 +- src/network/featureflags.h | 169 ++++++++++++++++++------------------- 3 files changed, 101 insertions(+), 93 deletions(-) diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 3195a143a..3f1acb67d 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -111,7 +111,8 @@ bool Connection::endPacket() { } bool Connection::beginBundle() { - MUST_TRANSFER_BOOL(m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)); + MUST_TRANSFER_BOOL(m_ServerFeatures.has(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT + )); memset(m_Bundle, 0, sizeof(m_Bundle)); m_BundleInnerPosition = m_Bundle; @@ -126,7 +127,8 @@ bool Connection::beginBundle() { } bool Connection::sendBundle() { - MUST_TRANSFER_BOOL(m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)); + MUST_TRANSFER_BOOL(m_ServerFeatures.has(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT + )); MUST_TRANSFER_BOOL(!m_IsBundle); MUST_TRANSFER_BOOL((m_BundlePacketCount > 0)); @@ -393,7 +395,8 @@ void Connection::sendFeatureFlags() { MUST(sendPacketType(PACKET_FEATURE_FLAGS)); MUST(sendPacketNumber()); - MUST(write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size())); + auto packedFeatures = m_FirmwareFeatures.pack(); + MUST(write(packedFeatures.data(), packedFeatures.size())); MUST(endPacket()); } @@ -592,7 +595,7 @@ void Connection::searchForServer() { m_Connected = true; m_FeatureFlagsRequestAttempts = 0; - m_ServerFeatures = ServerFeatures{}; + m_ServerFeatures = FeatureFlags(); statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false); ledManager.off(); @@ -716,11 +719,13 @@ void Connection::update() { bool hadFlags = m_ServerFeatures.isAvailable(); uint32_t flagsLength = len - 12; - m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength); + m_ServerFeatures + = FeatureFlags(&m_Packet[12], flagsLength); if (!hadFlags) { #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED - if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) { + if (m_ServerFeatures.has(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT + )) { m_Logger.debug("Server supports packet bundling"); } #endif diff --git a/src/network/connection.h b/src/network/connection.h index 0088c12b8..f9761505a 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -37,6 +37,9 @@ namespace Network { class Connection { public: + Connection() + : m_FirmwareFeatures(m_EnabledFirmwareFeatures) {} + void searchForServer(); void update(); void reset(); @@ -107,7 +110,7 @@ class Connection { ); #endif - const ServerFeatures& getServerFeatureFlags() { return m_ServerFeatures; } + const auto getServerFeatureFlags() { return m_ServerFeatures; } bool beginBundle(); bool endBundle(); @@ -166,7 +169,8 @@ class Connection { uint8_t m_FeatureFlagsRequestAttempts = 0; unsigned long m_FeatureFlagsRequestTimestamp = millis(); - ServerFeatures m_ServerFeatures{}; + FeatureFlags m_ServerFeatures; + FeatureFlags m_FirmwareFeatures; bool m_IsBundle = false; /* `53` is the maximum size of any packet that could be bundled, which is the diff --git a/src/network/featureflags.h b/src/network/featureflags.h index b0820be40..64148437b 100644 --- a/src/network/featureflags.h +++ b/src/network/featureflags.h @@ -1,103 +1,102 @@ + /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_FEATURE_FLAGS_H_ #define SLIMEVR_FEATURE_FLAGS_H_ -#include #include - -/** - * Bit packed flags, enum values start with 0 and indicate which bit it is. - * - * Change the enums and `flagsEnabled` inside to extend. -*/ -struct ServerFeatures { -public: - enum EServerFeatureFlags: uint32_t { - // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). - PROTOCOL_BUNDLE_SUPPORT, - - // Add new flags here - - BITS_TOTAL, - }; - - bool has(EServerFeatureFlags flag) { - uint32_t bit = static_cast(flag); - return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8))); - } - - /** - * Whether the server supports the "feature flags" feature, - * set to true when we've received flags packet from the server. - */ - bool isAvailable() { - return m_Available; - } - - static ServerFeatures from(uint8_t* received, uint32_t length) { - ServerFeatures res; - res.m_Available = true; - memcpy(res.m_Flags, received, std::min(static_cast(sizeof(res.m_Flags)), length)); - return res; - } - -private: - bool m_Available = false; - - uint8_t m_Flags[static_cast(EServerFeatureFlags::BITS_TOTAL) / 8 + 1]; +#include +#include +#include + +// I hate C++11 - they fixed this in C++14, but our compilers are old as the iceage +struct EnumClassHash { + template + std::size_t operator()(T t) const { + return static_cast(t); + } }; -class FirmwareFeatures { -public: - enum EFirmwareFeatureFlags: uint32_t { - // EXAMPLE_FEATURE, - B64_WIFI_SCANNING = 1, - - // Add new flags here +enum EServerFeatureFlags : uint32_t { + // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). + PROTOCOL_BUNDLE_SUPPORT, - BITS_TOTAL, - }; + BITS_TOTAL, +}; - // Flags to send - static constexpr const std::initializer_list flagsEnabled = { - // EXAMPLE_FEATURE, - B64_WIFI_SCANNING, +enum class EFirmwareFeatureFlags : uint32_t { + // EXAMPLE_FEATURE, + B64_WIFI_SCANNING = 1, - // Add enabled flags here - }; + BITS_TOTAL, +}; - static constexpr auto flags = []{ - constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1; - std::array packed{}; +static const std::unordered_map + m_EnabledFirmwareFeatures = {{EFirmwareFeatureFlags::B64_WIFI_SCANNING, true}}; - for (uint32_t bit : flagsEnabled) { - packed[bit / 8] |= 1 << (bit % 8); - } +template +class FeatureFlags { +public: + static constexpr auto FLAG_BYTES + = ((static_cast(Flags::BITS_TOTAL)) + 7) / 8; + + FeatureFlags() + : m_Available(false) {} + FeatureFlags(uint8_t* packed, uint32_t length) + : m_Available(true) { + for (uint32_t bit = 0; bit < length * 8; bit++) { + auto posInPacked = bit / 8; + auto posInByte = bit % 8; + + m_Flags[static_cast(bit)] = packed[posInPacked] & (1 << posInByte); + } + } + FeatureFlags(std::unordered_map flags) + : m_Available(true) + , m_Flags(flags) {} + + std::array pack() { + std::array packed{}; + + for (auto& [flag, value] : m_Flags) { + auto posInPacked = static_cast(flag) / 8; + auto posInByte = static_cast(flag) % 8; + + if (value) { + packed[posInPacked] |= 1 << posInByte; + } + } + + return packed; + }; + + bool has(Flags flag) { return m_Flags[flag]; } + bool isAvailable() { return m_Available; } - return packed; - }(); +private: + bool m_Available = false; + std::unordered_map m_Flags{}; }; #endif