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(); } } } }