diff --git a/src/main/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPoint.java b/src/main/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPoint.java index 42d69d722..eebf97228 100644 --- a/src/main/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPoint.java +++ b/src/main/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPoint.java @@ -32,6 +32,7 @@ import java.util.ArrayList; import java.util.List; import java.util.concurrent.FutureTask; +import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicInteger; public class UDPFacadeEndPoint extends EndPoint { @@ -40,6 +41,7 @@ public class UDPFacadeEndPoint extends EndPoint { private final EndPoint endPoint; private final SocketAddress fromAddress; + private final AtomicBoolean closed = new AtomicBoolean(); public UDPFacadeEndPoint(EndPoint endPoint, SocketAddress fromAddress, EndPointServerStatus server) { super(counter.incrementAndGet(), server); @@ -71,16 +73,25 @@ public String getProtocol() { @Override public int sendPacket(Packet packet) throws IOException { + if (closed.get()) { + throw new IOException("UDP facade endpoint is closed"); + } return endPoint.sendPacket(packet); } @Override public int readPacket(Packet packet) throws IOException { + if (closed.get()) { + return -1; + } return endPoint.readPacket(packet); } @Override public FutureTask register(int selectionKey, Selectable runner) throws IOException { + if (closed.get()) { + throw new ClosedChannelException(); + } return endPoint.register(selectionKey, runner); } @@ -112,7 +123,10 @@ public String getRemoteSocketAddress() { @Override public void close() throws IOException { - endPoint.close(); + if (!closed.compareAndSet(false, true)) { + return; + } + super.close(); EndPointServer endPointServer = (EndPointServer)endPoint.getServer(); endPointServer.handleCloseEndPoint(this); } diff --git a/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkInterfaceManager.java b/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkInterfaceManager.java index 1cd3c768a..869221bed 100644 --- a/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkInterfaceManager.java +++ b/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkInterfaceManager.java @@ -19,7 +19,6 @@ package io.mapsmessaging.network.protocol.impl.mavlink; -import static io.mapsmessaging.logging.ServerLogMessages.MAVLINK_DETECTED_PACKET; import static io.mapsmessaging.logging.ServerLogMessages.MAVLINK_FAILED_FORWARD_PACKET; import static io.mapsmessaging.logging.ServerLogMessages.MAVLINK_FAILED_PARSING_FORWARD_LIST; import static io.mapsmessaging.logging.ServerLogMessages.MAVLINK_FAILED_SETTING_UP_SESSION; @@ -30,7 +29,6 @@ import io.mapsmessaging.logging.Logger; import io.mapsmessaging.logging.LoggerFactory; import io.mapsmessaging.mavlink.MavlinkEventFactory; -import io.mapsmessaging.mavlink.ProcessedFrame; import io.mapsmessaging.mavlink.tlog.MavlinkTlogWriter; import io.mapsmessaging.mavlink.tlog.TlogConfiguration; import io.mapsmessaging.network.io.EndPoint; @@ -39,7 +37,6 @@ import io.mapsmessaging.network.io.impl.SelectorTask; import io.mapsmessaging.network.io.impl.udp.UDPFacadeEndPoint; import io.mapsmessaging.network.io.impl.udp.session.UDPSessionState; -import lombok.Getter; import java.io.IOException; import java.net.InetSocketAddress; @@ -52,7 +49,6 @@ import java.util.ArrayList; import java.util.List; import java.util.Locale; -import java.util.Optional; public class MavlinkInterfaceManager implements SelectorCallback, MavlinkConnectionManager { @@ -61,7 +57,6 @@ public class MavlinkInterfaceManager implements SelectorCallback, MavlinkConnect private final SelectorTask selectorTask; private final EndPoint endPoint; private final MavLinkSessionManager currentSessions; - private final MavlinkEventFactory mavlinkEventFactory; private final MavlinkConfig mavlinkConfig; private final List forwardList; private final MavlinkTlogWriter tlogWriter; @@ -70,7 +65,6 @@ public MavlinkInterfaceManager(EndPoint endPoint) throws IOException { this.endPoint = endPoint; mavlinkConfig = (MavlinkConfig) endPoint.getConfig().getProtocolConfig("mavlink"); long timeout = mavlinkConfig.getIdleSessionTimeout(); - mavlinkEventFactory = loadDialect(mavlinkConfig.getDialectName()); currentSessions = new MavLinkSessionManager<>(timeout); selectorTask = new SelectorTask(this, endPoint.getConfig().getEndPointConfig(), endPoint.isUDP()); selectorTask.register(SelectionKey.OP_READ); @@ -103,40 +97,50 @@ public static MavlinkEventFactory loadDialect(String name) throws IOException { @Override public boolean processPacket(Packet packet) throws IOException { - if (packet.getFromAddress() == null) { - return true; - } + try { + SocketAddress fromAddress = packet.getFromAddress(); + if (fromAddress == null) { + return true; + } + + byte[] raw = new byte[packet.available()]; + int pos = packet.position(); + packet.get(raw); + packet.position(pos); + + boolean forwardedSource = fromForward(fromAddress); + List packets = MavlinkFrameExtractor.extractMavlinkFrames(raw); + for(byte[] data:packets) { + writeTlog(data); + int systemId = MavlinkFrameExtractor.getSystemId(data); + if (!isAllowedSystem(systemId)) { + if (!forwardedSource) { + forwardPacket(data); + } + continue; + } - byte[] raw = new byte[packet.available()]; - int pos = packet.position(); - packet.get(raw); - packet.position(pos); - - List packets = MavlinkFrameExtractor.extractMavlinkFrames(raw); - for(byte[] data:packets) { - writeTlog(data); - int systemId = MavlinkFrameExtractor.getSystemId(data); - MavlinkDeviceKey key = buildKey(packet, systemId); - boolean allowed = - mavlinkConfig.getAcceptedSources() == null - || mavlinkConfig.getAcceptedSources().isEmpty() - || mavlinkConfig.getAcceptedSources().stream().anyMatch(knownSource -> knownSource.getSystemId() == key.getSystemId()); - - if (allowed) { + MavlinkDeviceKey key = buildKey(packet, systemId); UDPSessionState state = findOrCreate(key); - if (fromForward(packet)) { - state.getContext().processPacket(packet); - } else if (state.getContext() != null) { - MavlinkProtocol protocol = state.getContext(); - protocol.processRawFrame(data, packet.getFromAddress().toString()); + if (state == null || state.getContext() == null) { + continue; + } + + state.getContext().processRawFrame(data, fromAddress.toString()); + if (!forwardedSource) { forwardPacket(data); } - } else { - forwardPacket(data); } + return true; + } finally { + selectorTask.register(SelectionKey.OP_READ); } - selectorTask.register(SelectionKey.OP_READ); - return true; + } + + private boolean isAllowedSystem(int systemId) { + return mavlinkConfig.getAcceptedSources() == null + || mavlinkConfig.getAcceptedSources().isEmpty() + || mavlinkConfig.getAcceptedSources().stream().anyMatch(knownSource -> knownSource.getSystemId() == systemId); } private MavlinkTlogWriter createTlogWriter() throws IOException { @@ -171,22 +175,29 @@ private MavlinkDeviceKey buildKey(Packet packet, int systemId) { private synchronized UDPSessionState findOrCreate(MavlinkDeviceKey key) { UDPSessionState state = currentSessions.getState(key); - if (state == null) { - UDPFacadeEndPoint facade = new UDPFacadeEndPoint(endPoint, key.getRemoteAddress(), endPoint.getServer()); + if (state != null) { + return state; + } + + UDPFacadeEndPoint facade = new UDPFacadeEndPoint(endPoint, key.getRemoteAddress(), endPoint.getServer()); + try { + MavlinkProtocol protocol = new MavlinkProtocol(this, key, facade, this.mavlinkConfig); + state = new UDPSessionState<>(protocol); + currentSessions.addState(key, state); + logger.log(MAVLINK_SESSION_CREATED, key.toString()); + return state; + } catch (IOException | RuntimeException e) { try { - MavlinkProtocol protocol = new MavlinkProtocol(this, key, facade, this.mavlinkConfig); - state = new UDPSessionState<>(protocol); - currentSessions.addState(key, state); - logger.log(MAVLINK_SESSION_CREATED, key.toString()); - } catch (IOException e) { - logger.log(MAVLINK_FAILED_SETTING_UP_SESSION, key.toString(), e); + facade.close(); + } catch (IOException closeException) { + e.addSuppressed(closeException); } + logger.log(MAVLINK_FAILED_SETTING_UP_SESSION, key.toString(), e); + return null; } - return state; } - private boolean fromForward(Packet packet) { - SocketAddress fromAddress = packet.getFromAddress(); + private boolean fromForward(SocketAddress fromAddress) { return forwardList.stream().anyMatch(forwardAddress -> forwardAddress.equals(fromAddress)); } @@ -276,4 +287,4 @@ private static String toSafeFileName(String value) { return safe.substring(start, end); } -} \ No newline at end of file +} diff --git a/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkProtocol.java b/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkProtocol.java index 019059290..f928b9cc0 100644 --- a/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkProtocol.java +++ b/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkProtocol.java @@ -56,6 +56,7 @@ import io.mapsmessaging.schemas.formatters.MessageFormatterFactory; import java.io.IOException; import java.net.InetSocketAddress; +import java.net.SocketAddress; import java.nio.ByteBuffer; import java.nio.charset.StandardCharsets; import java.util.*; @@ -81,9 +82,9 @@ public class MavlinkProtocol extends Protocol { protected final MavlinkConfigDTO mavlinkConfig; protected Session session; private final Map acceptedComponents; - private final SequenceTracker tracker; + private final Map sequenceTrackers; private final String outboundTopicName; - protected MavlinkEventFactory mavlinkEventFactory; + protected volatile MavlinkEventFactory mavlinkEventFactory; protected final MessageFormatter formatter; private final QualityOfService qos; private final boolean storeOffline; @@ -102,7 +103,7 @@ protected MavlinkProtocol( super(endPoint, protocolConfig); this.factory = factory; this.key = key; - tracker = new SequenceTracker(); + sequenceTrackers = new ConcurrentHashMap<>(); this.mavlinkConfig = (MavlinkConfigDTO) protocolConfig; String dialectName = mavlinkConfig.getDialectName(); mavlinkEventFactory = MavlinkInterfaceManager.loadDialect(dialectName); @@ -145,23 +146,30 @@ protected MavlinkProtocol( session.addSubscription(subscriptionContextBuilder.build()); } outboundTopicName = outboundTopic; - String remoteSocket = endPoint.getRemoteSocketAddress(); - heartbeatEmitter = new MavlinkHeartbeatEmitter(sequenceCounter, endPoint, mavlinkConfig, parseSocketAddress(remoteSocket)); + if (mavlinkConfig.hasLocalMavlinkIdentity()) { + SocketAddress heartbeatAddress = endPoint.isUDP() ? parseSocketAddress(endPoint.getRemoteSocketAddress()) : null; + heartbeatEmitter = new MavlinkHeartbeatEmitter(sequenceCounter, endPoint, mavlinkConfig, heartbeatAddress); + } else { + heartbeatEmitter = null; + } startHeartbeatIfConfigured(); } @Override public void close() throws IOException { stopHeartbeat(); - if (!session.isClosed()) { - SessionManager.getInstance().close(session, false); - } - endPoint.close(); - if (mbean != null) { - mbean.close(); + try { + if (!session.isClosed()) { + SessionManager.getInstance().close(session, false); + } + endPoint.close(); + if (mbean != null) { + mbean.close(); + } + super.close(); + } finally { + factory.close(key); } - super.close(); - factory.close(key); } @Override @@ -205,14 +213,10 @@ public void sendMessage(@NotNull @NonNull MessageEvent messageEvent) { } String json = new String(messageEvent.getMessage().getOpaqueData(), StandardCharsets.UTF_8); - JsonObject input = JsonParser.parseString(json).getAsJsonObject(); - String socketAddressText = parts[2]; - sendData(input, socketAddressText); - } - catch(Throwable th){ - th.printStackTrace(); - } - finally { + sendData(JsonParser.parseString(json).getAsJsonObject(), parts[2]); + } catch (RuntimeException e) { + logger.log(MAVLINK_FAILED_SENDING_OUTBOUND_PACKET, endPoint.getName(), "message-event", e); + } finally { messageEvent.getCompletionTask().run(); } } @@ -223,10 +227,12 @@ private void sendData(JsonObject input, String socketAddressText) { validateOutboundHeader(input); byte[] frame = formatter.parseFromJson(input); Packet packet = new Packet(ByteBuffer.wrap(frame)); - packet.setFromAddress(parseSocketAddress(socketAddressText)); + if (endPoint.isUDP()) { + packet.setFromAddress(parseSocketAddress(socketAddressText)); + } endPoint.sendPacket(packet); factory.writeTlog(frame); - } catch (Throwable e) { + } catch (Exception e) { logger.log(MAVLINK_FAILED_SENDING_OUTBOUND_PACKET, endPoint.getName(), socketAddressText, e); } } @@ -277,6 +283,8 @@ public void processRawFrame(byte[] raw, String socketAddress) throws IOException } if (mavlinkConfig.getStatusTopicNameTemplate() != null && !mavlinkConfig.getStatusTopicNameTemplate().isEmpty()) { + int trackerKey = (env.getFrame().getSystemId() << 8) | env.getFrame().getComponentId(); + SequenceTracker tracker = sequenceTrackers.computeIfAbsent(trackerKey, ignored -> new SequenceTracker()); SequenceResult results = tracker.accept(env.getFrame().getSequence()); if (results.isStatusChanged()) { String statusTopic = computeTopicName(mavlinkConfig.getStatusTopicNameTemplate(), env.getFrame(), env.getMessageName()); @@ -297,7 +305,7 @@ public void processRawFrame(byte[] raw, String socketAddress) throws IOException if (env.getDetections() != null && !env.getDetections().isEmpty()) { envelope.add("detections", gson.toJsonTree(env.getDetections()).getAsJsonArray()); } - raw = envelope.toString().getBytes(); + raw = envelope.toString().getBytes(StandardCharsets.UTF_8); } processPacket(env.getFrame(), env.getMessageName(), raw, socketAddress); } else { @@ -460,18 +468,19 @@ private static InetSocketAddress parseSocketAddress(String socketAddressText) { private void startHeartbeatIfConfigured() { - if (!mavlinkConfig.hasLocalMavlinkIdentity()) { + if (heartbeatEmitter == null || heartbeatFuture != null) { return; } long intervalSeconds = Math.max(1, mavlinkConfig.getHeartbeatIntervalSeconds()); - SimpleTaskScheduler.getInstance().scheduleAtFixedRate(heartbeatEmitter, intervalSeconds, intervalSeconds, TimeUnit.SECONDS); + heartbeatFuture = SimpleTaskScheduler.getInstance().scheduleAtFixedRate(heartbeatEmitter, intervalSeconds, intervalSeconds, TimeUnit.SECONDS); } private void stopHeartbeat() { - if (heartbeatFuture != null) { - heartbeatFuture.cancel(false); - heartbeatFuture = null; + ScheduledFuture future = heartbeatFuture; + heartbeatFuture = null; + if (future != null) { + future.cancel(false); } } @@ -518,4 +527,4 @@ private int getRequiredUnsignedByte(JsonObject object, String fieldName) { return value; } -} \ No newline at end of file +} diff --git a/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandler.java b/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandler.java index 30368c40d..888542fc2 100644 --- a/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandler.java +++ b/src/main/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandler.java @@ -35,7 +35,7 @@ public class MavlinkStreamHandler implements StreamHandler { private static final int MAVLINK_V1_MAGIC = 0xFE; private static final int MAVLINK_V2_MAGIC = 0xFD; - private static final int MAVLINK_V1_HEADER_REST = 5; // after magic+len + private static final int MAVLINK_V1_HEADER_REST = 4; // seq, sysid, compid, msgid private static final int MAVLINK_V1_CRC_LEN = 2; private static final int MAVLINK_V2_HEADER_REST = 8; // after magic+len (incompat, compat, seq, sys, comp, msgid[3]) @@ -43,7 +43,8 @@ public class MavlinkStreamHandler implements StreamHandler { private static final int MAVLINK_V2_SIGNATURE_LEN = 13; private static final int MAVLINK_V2_INCOMPAT_FLAG_SIGNED = 0x01; - private final byte[] smallBuffer; + private final byte[] inputBuffer; + private final byte[] outputBuffer; private final int readTimeoutMillis; private volatile boolean closed; @@ -52,7 +53,8 @@ public MavlinkStreamHandler() { this(DEFAULT_READ_TIMEOUT_MILLIS); } public MavlinkStreamHandler(int readTimeoutMillis) { - this.smallBuffer = new byte[32]; + this.inputBuffer = new byte[32]; + this.outputBuffer = new byte[32]; this.closed = false; if (readTimeoutMillis <= 0) { throw new IllegalArgumentException("readTimeoutMillis must be > 0"); @@ -106,19 +108,19 @@ public int parseInput(InputStream input, Packet packet) throws IOException { packet.putByte(magic); packet.putByte(payloadLength); - readFully(input, smallBuffer, 0, MAVLINK_V1_HEADER_REST); - packet.put(smallBuffer, 0, MAVLINK_V1_HEADER_REST); + readFully(input, inputBuffer, 0, MAVLINK_V1_HEADER_REST); + packet.put(inputBuffer, 0, MAVLINK_V1_HEADER_REST); readPayload(input, packet, payloadLength); - readFully(input, smallBuffer, 0, MAVLINK_V1_CRC_LEN); - packet.put(smallBuffer, 0, MAVLINK_V1_CRC_LEN); + readFully(input, inputBuffer, 0, MAVLINK_V1_CRC_LEN); + packet.put(inputBuffer, 0, MAVLINK_V1_CRC_LEN); return frameLength; } - readFully(input, smallBuffer, 0, MAVLINK_V2_HEADER_REST); + readFully(input, inputBuffer, 0, MAVLINK_V2_HEADER_REST); - int incompatFlags = smallBuffer[0] & 0xFF; + int incompatFlags = inputBuffer[0] & 0xFF; boolean signed = (incompatFlags & MAVLINK_V2_INCOMPAT_FLAG_SIGNED) != 0; int frameLength = 2 + MAVLINK_V2_HEADER_REST + payloadLength + MAVLINK_V2_CRC_LEN + (signed ? MAVLINK_V2_SIGNATURE_LEN : 0); @@ -127,16 +129,16 @@ public int parseInput(InputStream input, Packet packet) throws IOException { packet.putByte(magic); packet.putByte(payloadLength); - packet.put(smallBuffer, 0, MAVLINK_V2_HEADER_REST); + packet.put(inputBuffer, 0, MAVLINK_V2_HEADER_REST); readPayload(input, packet, payloadLength); - readFully(input, smallBuffer, 0, MAVLINK_V2_CRC_LEN); - packet.put(smallBuffer, 0, MAVLINK_V2_CRC_LEN); + readFully(input, inputBuffer, 0, MAVLINK_V2_CRC_LEN); + packet.put(inputBuffer, 0, MAVLINK_V2_CRC_LEN); if (signed) { - readFully(input, smallBuffer, 0, MAVLINK_V2_SIGNATURE_LEN); - packet.put(smallBuffer, 0, MAVLINK_V2_SIGNATURE_LEN); + readFully(input, inputBuffer, 0, MAVLINK_V2_SIGNATURE_LEN); + packet.put(inputBuffer, 0, MAVLINK_V2_SIGNATURE_LEN); } return frameLength; } @@ -168,9 +170,9 @@ public int parseOutput(OutputStream output, Packet packet) throws IOException { } while (buffer.hasRemaining()) { - int chunk = Math.min(buffer.remaining(), smallBuffer.length); - buffer.get(smallBuffer, 0, chunk); - output.write(smallBuffer, 0, chunk); + int chunk = Math.min(buffer.remaining(), outputBuffer.length); + buffer.get(outputBuffer, 0, chunk); + output.write(outputBuffer, 0, chunk); } return length; @@ -188,9 +190,9 @@ private void readPayload(InputStream input, Packet packet, int payloadLength) th } int remaining = payloadLength; while (remaining > 0) { - int chunk = Math.min(remaining, smallBuffer.length); - readFully(input, smallBuffer, 0, chunk); - packet.put(smallBuffer, 0, chunk); + int chunk = Math.min(remaining, inputBuffer.length); + readFully(input, inputBuffer, 0, chunk); + packet.put(inputBuffer, 0, chunk); remaining -= chunk; } } diff --git a/src/test/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPointTest.java b/src/test/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPointTest.java new file mode 100644 index 000000000..ba6257d32 --- /dev/null +++ b/src/test/java/io/mapsmessaging/network/io/impl/udp/UDPFacadeEndPointTest.java @@ -0,0 +1,65 @@ +/* + * + * Copyright [ 2020 - 2024 ] Matthew Buckton + * Copyright [ 2024 - 2026 ] MapsMessaging B.V. + * + * Licensed under the Apache License, Version 2.0 with the Commons Clause + * (the "License"); you may not use this file except in compliance with the License. + */ + +package io.mapsmessaging.network.io.impl.udp; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.times; +import static org.mockito.Mockito.verify; +import static org.mockito.Mockito.when; + +import io.mapsmessaging.network.io.EndPoint; +import io.mapsmessaging.network.io.EndPointServer; +import io.mapsmessaging.network.io.Packet; +import java.io.IOException; +import java.net.InetSocketAddress; +import java.util.List; +import org.junit.jupiter.api.Test; + +class UDPFacadeEndPointTest { + + @Test + void closeIsLogicalAndDoesNotCloseSharedPhysicalEndpoint() throws Exception { + EndPoint physical = mock(EndPoint.class); + EndPointServer server = mock(EndPointServer.class); + when(physical.getJMXTypePath()).thenReturn(List.of("server", "endpoint")); + when(physical.getName()).thenReturn("udp-server"); + when(physical.getServer()).thenReturn(server); + UDPFacadeEndPoint facade = new UDPFacadeEndPoint(physical, new InetSocketAddress("127.0.0.1", 14550), server); + + facade.close(); + facade.close(); + + verify(physical, never()).close(); + verify(server, times(1)).handleCloseEndPoint(facade); + } + + @Test + void closedFacadeRejectsTrafficWithoutTouchingPhysicalEndpoint() throws Exception { + EndPoint physical = mock(EndPoint.class); + EndPointServer server = mock(EndPointServer.class); + Packet packet = new Packet(32, false); + when(physical.getJMXTypePath()).thenReturn(List.of("server", "endpoint")); + when(physical.getName()).thenReturn("udp-server"); + when(physical.getServer()).thenReturn(server); + when(physical.sendPacket(packet)).thenReturn(7); + UDPFacadeEndPoint facade = new UDPFacadeEndPoint(physical, new InetSocketAddress("127.0.0.1", 14550), server); + + assertEquals(7, facade.sendPacket(packet)); + facade.close(); + + assertThrows(IOException.class, () -> facade.sendPacket(packet)); + assertEquals(-1, facade.readPacket(packet)); + verify(physical, times(1)).sendPacket(packet); + verify(physical, never()).readPacket(packet); + } +} diff --git a/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkFrameExtractorTest.java b/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkFrameExtractorTest.java new file mode 100644 index 000000000..b54bd03a5 --- /dev/null +++ b/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkFrameExtractorTest.java @@ -0,0 +1,46 @@ +/* + * + * Copyright [ 2020 - 2024 ] Matthew Buckton + * Copyright [ 2024 - 2026 ] MapsMessaging B.V. + * + * Licensed under the Apache License, Version 2.0 with the Commons Clause + * (the "License"); you may not use this file except in compliance with the License. + */ + +package io.mapsmessaging.network.protocol.impl.mavlink; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.List; +import org.junit.jupiter.api.Test; + +class MavlinkFrameExtractorTest { + + @Test + void extractsBackToBackV1FramesUsingPayloadPlusEightBytes() { + byte[] first = {(byte) 0xFE, 2, 7, 11, 3, 42, 90, 91, 12, 13}; + byte[] second = {(byte) 0xFE, 1, 8, 12, 4, 43, 92, 14, 15}; + byte[] input = new byte[first.length + second.length]; + System.arraycopy(first, 0, input, 0, first.length); + System.arraycopy(second, 0, input, first.length, second.length); + + List frames = MavlinkFrameExtractor.extractMavlinkFrames(input); + + assertEquals(2, frames.size()); + assertArrayEquals(first, frames.get(0)); + assertArrayEquals(second, frames.get(1)); + assertEquals(11, MavlinkFrameExtractor.getSystemId(first)); + } + + @Test + void ignoresNoiseAndLeavesTruncatedTrailingFrameUnconsumed() { + byte[] complete = {(byte) 0xFE, 0, 1, 21, 2, 0, 10, 11}; + byte[] input = {99, complete[0], complete[1], complete[2], complete[3], complete[4], complete[5], complete[6], complete[7], (byte) 0xFE, 4, 1}; + + List frames = MavlinkFrameExtractor.extractMavlinkFrames(input); + + assertEquals(1, frames.size()); + assertArrayEquals(complete, frames.getFirst()); + } +} diff --git a/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerConcurrencyTest.java b/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerConcurrencyTest.java new file mode 100644 index 000000000..b9847d99e --- /dev/null +++ b/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerConcurrencyTest.java @@ -0,0 +1,135 @@ +/* + * + * Copyright [ 2020 - 2024 ] Matthew Buckton + * Copyright [ 2024 - 2026 ] MapsMessaging B.V. + * + * Licensed under the Apache License, Version 2.0 with the Commons Clause + * (the "License"); you may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.apache.org/licenses/LICENSE-2.0 + * https://commonsclause.com/ + */ + +package io.mapsmessaging.network.protocol.impl.mavlink; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import io.mapsmessaging.network.io.Packet; +import java.io.ByteArrayOutputStream; +import java.io.IOException; +import java.io.InputStream; +import java.nio.ByteBuffer; +import java.util.concurrent.CountDownLatch; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; +import org.junit.jupiter.api.Test; + +class MavlinkStreamHandlerConcurrencyTest { + + @Test + void inputAndOutputUseIndependentScratchBuffers() throws Exception { + byte[] frame = { + (byte) 0xFE, + 4, + 1, + 2, + 3, + 4, + 10, + 11, + 12, + 13, + 20, + 21 + }; + + CountDownLatch headerCopied = new CountDownLatch(1); + CountDownLatch outputCompleted = new CountDownLatch(1); + InputStream input = + new CoordinatedInputStream(frame, headerCopied, outputCompleted); + MavlinkStreamHandler handler = new MavlinkStreamHandler(2_000); + Packet inbound = new Packet(64, false); + + ExecutorService executor = Executors.newSingleThreadExecutor(); + try { + Future read = executor.submit(() -> handler.parseInput(input, inbound)); + assertTrue(headerCopied.await(1, TimeUnit.SECONDS)); + + Packet outbound = + new Packet(ByteBuffer.wrap(new byte[] {85, 85, 85, 85, 85, 85, 85, 85})); + ByteArrayOutputStream output = new ByteArrayOutputStream(); + try { + assertEquals(8, handler.parseOutput(output, outbound)); + } finally { + outputCompleted.countDown(); + } + + assertEquals(frame.length, read.get(1, TimeUnit.SECONDS)); + inbound.flip(); + byte[] actual = new byte[inbound.available()]; + inbound.get(actual); + + assertArrayEquals(frame, actual); + assertArrayEquals(new byte[] {85, 85, 85, 85, 85, 85, 85, 85}, output.toByteArray()); + } finally { + outputCompleted.countDown(); + executor.shutdownNow(); + } + } + + private static final class CoordinatedInputStream extends InputStream { + + private final byte[] data; + private final CountDownLatch headerCopied; + private final CountDownLatch outputCompleted; + private int position; + private boolean firstBulkRead = true; + + private CoordinatedInputStream( + byte[] data, + CountDownLatch headerCopied, + CountDownLatch outputCompleted) { + this.data = data; + this.headerCopied = headerCopied; + this.outputCompleted = outputCompleted; + } + + @Override + public int read() { + if (position >= data.length) { + return -1; + } + return data[position++] & 0xFF; + } + + @Override + public int read(byte[] buffer, int offset, int length) throws IOException { + if (position >= data.length) { + return -1; + } + + int count = Math.min(length, data.length - position); + System.arraycopy(data, position, buffer, offset, count); + position += count; + + if (firstBulkRead) { + firstBulkRead = false; + headerCopied.countDown(); + try { + if (!outputCompleted.await(1, TimeUnit.SECONDS)) { + throw new IOException("Timed out waiting for concurrent output"); + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + throw new IOException("Interrupted waiting for concurrent output", e); + } + } + return count; + } + } +} diff --git a/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerTest.java b/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerTest.java index fdcb5ea93..3026fe9b0 100644 --- a/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerTest.java +++ b/src/test/java/io/mapsmessaging/network/protocol/impl/mavlink/MavlinkStreamHandlerTest.java @@ -55,7 +55,7 @@ class MavlinkStreamHandlerTest { private static final int MAVLINK_V1_MAGIC = 0xFE; private static final int MAVLINK_V2_MAGIC = 0xFD; - private static final int MAVLINK_V1_HEADER_REST = 5; + private static final int MAVLINK_V1_HEADER_REST = 4; private static final int MAVLINK_V1_CRC_LEN = 2; private static final int MAVLINK_V2_HEADER_REST = 8; @@ -316,7 +316,6 @@ private static byte[] buildV1Frame(int payloadLength, byte seq, byte sysId, byte bb.put(sysId); bb.put(compId); bb.put(msgId); - bb.put((byte) 0x00); bb.put(payload); bb.put(crc);