bool CommObject::ReceiveFrame(gsl::span<std::uint8_t> buffer, CommFrame& frame)
{
if (buffer.size() < 2)
{
return false;
}
bool status = this->SendCommandWithResponse(CommReceiver, ReceiverGetFrame, buffer.subspan(0, 2));
if (!status)
{
return status;
}
Reader reader(buffer.subspan(0, 2));
auto size = reader.ReadWordLE();
buffer = ReceiveSpan(size, buffer);
status = this->SendCommandWithResponse(CommReceiver, ReceiverGetFrame, buffer);
if (!status)
{
return status;
}
reader.Initialize(buffer);
const auto fullSize = reader.ReadWordLE();
const auto doppler = reader.ReadWordLE();
const auto rssi = reader.ReadWordLE();
gsl::span<std::uint8_t> frameContent;
if (!reader.Status())
{
LOG(LOG_LEVEL_ERROR, "[comm] Failed to receive frame");
}
else
{
LOGF(LOG_LEVEL_DEBUG, "[comm] Received frame %d bytes", static_cast<int>(fullSize));
auto span = reader.ReadArray(reader.RemainingSize());
frameContent = gsl::span<std::uint8_t>(const_cast<std::uint8_t*>(span.data()), span.size());
}
frame = CommFrame(doppler, rssi, fullSize, std::move(frameContent));
// if (fullSize == 0 || (doppler & 0xf000) != 0 || (rssi & 0xf000) != 0)
// {
// LOGF(LOG_LEVEL_ERROR, "[comm] Received invalid frame. Size: %d, Doppler: 0x%X, RSSI: 0x%X. ", fullSize, doppler, rssi);
// return false;
// }
return status;
}
static gsl::span<std::uint8_t> ReceiveSpan(std::uint16_t frameSize, gsl::span<std::uint8_t> buffer)
{
if (buffer.size() <= frameSize + 6)
{
return buffer;
}
else
{
return buffer.subspan(0, frameSize + 6);
}
}
void NetworkService::receivePacket(gsl::span<gsl::byte> received, std::string* error)
{
if (error) {
std::cout << "Error receiving packet: " << (*error) << std::endl;
// Find the owner of this remote endpoint
for (auto& conn : pimpl->activeConnections) {
if (conn.second->matchesEndpoint(pimpl->remoteEndpoint)) {
conn.second->setError(*error);
conn.second->close();
}
}
return;
}
if (received.size_bytes() == 0) {
return;
}
// Read connection id
short id = -1;
std::array<unsigned char, 2> bytes;
auto dst = gsl::as_writeable_bytes(gsl::span<unsigned char, 2>(bytes));
dst[0] = received[0];
if (bytes[0] & 0x80) {
if (received.size_bytes() < 2) {
// Invalid header
std::cout << "Invalid header\n";
return;
}
dst[1] = received[1];
received = received.subspan(2);
id = short(bytes[0] & 0x7F) | short(bytes[1]);
} else {
received = received.subspan(1);
id = short(bytes[0]);
}
// No connection id, check if it's a connection request
if (id == 0 && isValidConnectionRequest(received)) {
auto& pending = pimpl->pendingIncomingConnections;
if (std::find(pending.begin(), pending.end(), pimpl->remoteEndpoint) == pending.end()) {
pending.push_back(pimpl->remoteEndpoint);
}
// Pending connection is valid
return;
}
// Find the owner of this remote endpoint
auto conn = pimpl->activeConnections.find(id);
if (conn == pimpl->activeConnections.end()) {
// Connection doesn't exist, but check the pending slot
conn = pimpl->activeConnections.find(0);
if (conn == pimpl->activeConnections.end()) {
// Nope, give up
return;
}
}
// Validate that this connection is who it claims to be
if (conn->second->matchesEndpoint(pimpl->remoteEndpoint)) {
auto connection = conn->second;
if (error) {
// Close the connection if there was an error
connection->setError(*error);
connection->close();
} else {
try {
connection->onReceive(received);
if (conn->first == 0) {
// Hold on, we're still on 0, re-bind to the id
short newId = connection->getConnectionId();
if (newId != 0) {
pimpl->activeConnections[newId] = connection;
pimpl->activeConnections.erase(conn);
}
}
} catch (std::exception& e) {
connection->setError(e.what());
connection->close();
} catch (...) {
connection->setError("Unknown error receiving packet.");
connection->close();
}
}
}
}
