Skip to content

Commit

Permalink
refactor(network): clean up packet bundling
Browse files Browse the repository at this point in the history
  • Loading branch information
@TheDevMinerTV
TheDevMinerTV committed Oct 8, 2023
1 parent cf12a50 commit 27b879a
Show file tree
Hide file tree
Showing 2 changed files with 119 additions and 75 deletions.
148 changes: 87 additions & 61 deletions src/network/connection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,22 @@ T convert_chars(unsigned char* const src) {
return un.v;
}

// taken from https://graphics.standford.edu/~seander/bithacks.html#BitReverseObvious
void reverse_bytes(uint8_t* buf) {
uint8_t v = *buf;
uint8_t r = v; // r will be reversed bits of v; first get LSB of v
int s = sizeof(v) * 8 - 1; // extra shift needed at end

for (v >>= 1; v; v >>= 1) {
r <<= 1;
r |= v & 1;
s--;
}
r <<= s; // shift when v's highest bits are zero

*buf = r;
}

namespace SlimeVR {
namespace Network {

Expand All @@ -67,7 +83,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 +104,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;
m_BundlePacketCount++;

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

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

Expand All @@ -115,39 +128,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();

return endPacket();
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) {
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);

Expand All @@ -156,13 +197,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;
Expand Down Expand Up @@ -197,18 +238,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 @@ -390,16 +425,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));
Expand Down Expand Up @@ -435,19 +470,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 @@ -618,13 +653,14 @@ void Connection::update() {
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);

Expand All @@ -647,17 +683,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
46 changes: 32 additions & 14 deletions src/network/connection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,11 @@
#include <Arduino.h>
#include <WiFiUdp.h>

#include "featureflags.h"
#include "globals.h"
#include "quat.h"
#include "sensors/sensor.h"
#include "wifihandler.h"
#include "featureflags.h"

namespace SlimeVR {
namespace Network {
Expand Down Expand Up @@ -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<Sensor *> & sensors);
void updateSensorState(std::vector<Sensor*>& 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);
Expand All @@ -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;
Expand All @@ -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
Expand Down

0 comments on commit 27b879a

Please sign in to comment.