Skip to content

Clean up packet bundles and feature flags #294

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Clean up packet bundles and feature flags #294

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a2f5a1e
refactor(network): use correct int*_t types for `send*` in NetworkCon…
TheDevMinerTV Aug 3, 2023
33fe59d
refactor(network): clean up packet bundling
TheDevMinerTV Aug 4, 2023
78d4a1d
refactor: feature flags
TheDevMinerTV Oct 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
192 changes: 107 additions & 85 deletions src/network/connection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -84,21 +88,14 @@ bool Connection::beginPacket() {

bool Connection::endPacket() {
if (m_IsBundle) {
uint32_t innerPacketSize = m_BundlePacketPosition;
auto innerPacketSize = getBundleInnerSize();

MUST_TRANSFER_BOOL((innerPacketSize > 0));
// 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);

m_IsBundle = false;

if (m_BundlePacketInnerCount == 0) {
sendPacketType(PACKET_BUNDLE);
sendPacketNumber();
}
sendShort(innerPacketSize);
sendBytes(m_Packet, innerPacketSize);
m_BundlePacketCount++;

m_BundlePacketInnerCount++;
m_IsBundle = true;
return true;
}

Expand All @@ -114,40 +111,68 @@ 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;
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(EServerFeatureFlags::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));

return endPacket();
auto ret = sendBundle();

memset(m_Buf, 0, sizeof(m_Buf));
m_BundleInnerStart = m_Buf;
m_BundleInnerPosition = m_Buf;
m_BundlePacketCount = 0;

return ret;
}

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)) {
if (getBundleSize() + size > MAX_BUNDLE_SIZE) {
m_Logger.error("Bundled packet too large");

// TODO: Drop the currently forming packet
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Clean up and reset of the buffer?
I think we should check for length before we even add more bytes, so we could send a packetbundle, and open a new one for the one that did not fit.


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);
return m_UDP.write(buffer, size);
}

bool Connection::sendFloat(float f) {
Expand All @@ -158,19 +183,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::sendU16(uint16_t i) {
convert_to_chars(i, m_Buf);

return write(m_Buf, sizeof(i)) != 0;
}

bool Connection::sendInt(uint32_t i) {
bool Connection::sendI32(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;
Expand All @@ -185,9 +210,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) {
Expand All @@ -199,18 +224,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);
}
Expand Down Expand Up @@ -376,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());
}
Expand All @@ -391,17 +411,17 @@ void Connection::sendTrackerDiscovery() {

MUST(sendPacketType(PACKET_HANDSHAKE));
// Packet number is always 0 for handshake
MUST(sendLong(0));
MUST(sendInt(BOARD));
MUST(sendU64(0));
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));
Expand Down Expand Up @@ -437,19 +457,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());
Expand Down Expand Up @@ -511,7 +531,7 @@ void Connection::returnLastPacket(int len) {
MUST(endPacket());
}

void Connection::updateSensorState(std::vector<Sensor *> & sensors) {
void Connection::updateSensorState(std::vector<Sensor*>& sensors) {
if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
return;
}
Expand All @@ -525,7 +545,7 @@ void Connection::updateSensorState(std::vector<Sensor *> & sensors) {
}
}

void Connection::maybeRequestFeatureFlags() {
void Connection::maybeRequestFeatureFlags() {
if (m_ServerFeatures.isAvailable() || m_FeatureFlagsRequestAttempts >= 15) {
return;
}
Expand Down Expand Up @@ -557,6 +577,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
Expand All @@ -571,9 +593,9 @@ void Connection::searchForServer() {
m_ServerPort = m_UDP.remotePort();
m_LastPacketTimestamp = millis();
m_Connected = true;

m_FeatureFlagsRequestAttempts = 0;
m_ServerFeatures = ServerFeatures { };
m_ServerFeatures = FeatureFlags<EServerFeatureFlags>();

statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
ledManager.off();
Expand Down Expand Up @@ -603,29 +625,38 @@ 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);

statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
}

void Connection::update() {
std::vector<Sensor *> & sensors = sensorManager.getSensors();
std::vector<Sensor*>& sensors = sensorManager.getSensors();

updateSensorState(sensors);
maybeRequestFeatureFlags();

if (!m_Connected) {
searchForServer();
return;
}

maybeRequestFeatureFlags();

if (m_LastPacketTimestamp + TIMEOUT < millis()) {
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;
Expand All @@ -639,17 +670,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<int>(m_Packet)) {
case PACKET_RECEIVE_HEARTBEAT:
Expand Down Expand Up @@ -697,16 +718,17 @@ void Connection::update() {
}

bool hadFlags = m_ServerFeatures.isAvailable();

uint32_t flagsLength = len - 12;
m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength);
m_ServerFeatures
= FeatureFlags<EServerFeatureFlags>(&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(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT
)) {
m_Logger.debug("Server supports packet bundling");
}
#endif
}

break;
Expand Down
Loading