From 1e95c4b8a69b56ed6be2576c1b6549f7dc15c5ff Mon Sep 17 00:00:00 2001 From: Swarup Nataraj Date: Wed, 5 Jun 2024 13:59:00 +0530 Subject: [PATCH] Add a new Packet decoder that can handle multiple packets in a frame with no segmentation --- .../ccsds/PixxelPacketMultipleDecoder.java | 206 ++++++++++++++++++ .../ccsds/VcDownlinkManagedParameters.java | 12 +- .../yamcs/tctm/ccsds/VcTmPacketHandler.java | 34 ++- 3 files changed, 246 insertions(+), 6 deletions(-) create mode 100644 yamcs-core/src/main/java/org/yamcs/tctm/ccsds/PixxelPacketMultipleDecoder.java diff --git a/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/PixxelPacketMultipleDecoder.java b/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/PixxelPacketMultipleDecoder.java new file mode 100644 index 00000000000..7c29c7b2010 --- /dev/null +++ b/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/PixxelPacketMultipleDecoder.java @@ -0,0 +1,206 @@ +package org.yamcs.tctm.ccsds; + +import java.util.function.Consumer; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.yamcs.tctm.PacketTooLongException; +import org.yamcs.tctm.TcTmException; +import org.yamcs.utils.ByteArrayUtils; + +/** + * Receives data chunk by chunk and assembles it into packets. Two types of + * packets are supported: + *

+ * Space Packet Protocol CCSDS 133.0-B-1 September 2003. + *

+ * The first 3 bits of these packets are 000. + *

+ * The header is 6 bytes and the last two bytes of the header represent the + * size of the packet (including the header) - 7. + *

+ * The minimum packet length is 7 bytes. + * + *

+ * Encapsulation Service. CCSDS 133.1-B-2. October 2009. + *

+ * The first 3 bits of these packets are 111. + *

+ * The minimum packet length is 1 byte. + *

+ * Depending on the last 2 bits of the first byte, the size of the header can be + * 1,2,4 or 8 bytes with the length of the + * packet read from the last 0,1,2 or 4 header bytes respectively + * + *

+ * The two types can be both present on the same stream. + * + *

+ * The objects of this class can processes one "stream" at a time and they are + * not thread safe! + * + * @author nm + * + */ +public class PixxelPacketMultipleDecoder { + private final int maxPacketLength; + + static final int PACKET_VERSION_CCSDS = 0; + + // the actual header length (valid when the headerOffset>0) + private int headerLength; + // max header length for the encapsulation packets is 8 bytes + final byte[] header = new byte[8]; + + private int headerOffset; + + // the packetOffset and packet will be valid when the header is completely read + // (i.e. + // headerOffset==header.length==lengthFieldEndOffset) + private int packetOffset; + private byte[] packet; + + final Consumer consumer; + + private boolean skipIdlePackets = true; + + final static byte[] ZERO_BYTES = new byte[0]; + static Logger log = LoggerFactory.getLogger(PixxelPacketMultipleDecoder.class.getName()); + + public PixxelPacketMultipleDecoder(int maxPacketLength, Consumer consumer) { + this.maxPacketLength = maxPacketLength; + this.consumer = consumer; + } + + public void process(byte[] data, int offset, int length) throws TcTmException, ArrayIndexOutOfBoundsException { + while (length > 0) { + if (headerOffset == 0) { // read the first byte of the header to know what kind of packet it is as well + // as + byte d0 = data[offset]; + offset++; + length--; + try { + headerLength = getHeaderLength(d0); + } catch (UnsupportedPacketVersionException e) { + break; + } + header[0] = d0; + headerOffset++; + + } else if (headerOffset < headerLength) { // reading the header + int n = Math.min(length, headerLength - headerOffset); + System.arraycopy(data, offset, header, headerOffset, n); + offset += n; + headerOffset += n; + length -= n; + if (headerOffset == headerLength) { + allocatePacket(); + } + } else {// reading the packet + int n = packet.length - packetOffset; + System.arraycopy(data, offset, packet, packetOffset, n); + offset += n; + packetOffset += n; + length -= n; + if (packetOffset == packet.length) { + sendToConsumer(); + packet = null; + headerOffset = 0; + } + } + } + } + + private static boolean isIdle(byte[] header) { + int b0 = header[0] & 0xFF; + int pv = b0 >>> 5; + + if (pv == PACKET_VERSION_CCSDS) { + return ((ByteArrayUtils.decodeUnsignedShort(header, 0) & 0x7FF) == 0x7FF); + } else { + return ((b0 & 0x1C) == 0); + } + } + + private void sendToConsumer() { + if (!skipIdlePackets || !isIdle(header)) { + consumer.accept(packet); + } else { + log.trace("skiping idle packet of size {}", packet.length); + } + } + + // get headerLength based on the first byte of the packet + private static int getHeaderLength(byte b0) throws UnsupportedPacketVersionException { + int pv = (b0 & 0xFF) >>> 5; + if (pv == PACKET_VERSION_CCSDS) { + return 6; + } else { + throw new UnsupportedPacketVersionException(pv); + } + } + + private void allocatePacket() throws TcTmException { + int packetLength = getPacketLength(header); + if (packetLength > maxPacketLength) { + throw new PacketTooLongException(maxPacketLength, packetLength); + } else if (packetLength < headerLength) { + throw new TcTmException( + "Invalid packet length " + packetLength + " (it is smaller than the header length)"); + } + packet = new byte[packetLength]; + System.arraycopy(header, 0, packet, 0, headerLength); + if (packetLength == headerLength) { + sendToConsumer(); + headerOffset = 0; + } else { + packetOffset = headerLength; + } + } + + // decodes the packet length from the header + private static int getPacketLength(byte[] header) throws UnsupportedPacketVersionException { + int h0 = header[0] & 0xFF; + int pv = h0 >>> 5; + if (pv == PACKET_VERSION_CCSDS) { + return 7 + ByteArrayUtils.decodeUnsignedShort(header, 4); + } else { + throw new UnsupportedPacketVersionException(pv); + } + } + + /** + * Removes a partial packet if any + */ + public void reset() { + headerOffset = 0; + packet = null; + } + + /** + * + * @return true of the decoder is in the middle of a packet decoding + */ + public boolean hasIncompletePacket() { + return (headerOffset > 0) && ((headerOffset < headerLength) || (packetOffset < packet.length)); + } + + /** + * + * @return true of the idle packets are skipped (i.e. not sent to the consumer) + */ + public boolean skipIdlePackets() { + return skipIdlePackets; + } + + /** + * Skip or not the idle packets. If true (default), the idle packets are not + * sent to the consumer. + * + * @param skipIdlePackets + */ + public void skipIdlePackets(boolean skipIdlePackets) { + this.skipIdlePackets = skipIdlePackets; + } + +} diff --git a/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcDownlinkManagedParameters.java b/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcDownlinkManagedParameters.java index baa2fd7110a..206a8b8ba1e 100644 --- a/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcDownlinkManagedParameters.java +++ b/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcDownlinkManagedParameters.java @@ -9,10 +9,16 @@ * */ public class VcDownlinkManagedParameters { + protected enum TMDecoder { + CCSDS, + SINGLE, + MULTIPLE; + } + protected int vcId; //if set to true, the encapsulation packets sent to the preprocessor will be without the encapsulation header(CCSDS 133.1-B-2) boolean stripEncapsulationHeader; - boolean usePixxelDecoder; + TMDecoder tmDecoder; // if service = M_PDU int maxPacketLength; @@ -24,13 +30,13 @@ public class VcDownlinkManagedParameters { public VcDownlinkManagedParameters(int vcId) { this.vcId = vcId; this.config = null; - usePixxelDecoder = false; + tmDecoder = TMDecoder.CCSDS; } public VcDownlinkManagedParameters(YConfiguration config) { this.config = config; this.vcId = config.getInt("vcId"); - usePixxelDecoder = config.getBoolean("usePixxelDecoder", false); + tmDecoder = config.getEnum("tmDecoder", TMDecoder.class, TMDecoder.MULTIPLE); } diff --git a/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcTmPacketHandler.java b/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcTmPacketHandler.java index 7c2ca08b137..adf57f835f4 100644 --- a/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcTmPacketHandler.java +++ b/yamcs-core/src/main/java/org/yamcs/tctm/ccsds/VcTmPacketHandler.java @@ -12,6 +12,7 @@ import org.yamcs.tctm.TcTmException; import org.yamcs.tctm.TmPacketDataLink; import org.yamcs.tctm.TmSink; +import org.yamcs.tctm.ccsds.VcDownlinkManagedParameters.TMDecoder; import org.yamcs.time.Instant; import org.yamcs.time.TimeService; import org.yamcs.utils.StringConverter; @@ -33,6 +34,7 @@ public class VcTmPacketHandler implements TmPacketDataLink, VcDownlinkHandler { private final Log log; PacketDecoder packetDecoder; PixxelPacketDecoder pPacketDecoder; + PixxelPacketMultipleDecoder pMultipleDecoder; long idleFrameCount = 0; PacketPreprocessor packetPreprocessor; final String name; @@ -53,6 +55,7 @@ public VcTmPacketHandler(String yamcsInstance, String name, VcDownlinkManagedPar // Temporary Packet Decoder pPacketDecoder = new PixxelPacketDecoder(vmp.maxPacketLength, p -> handlePacket(p)); + pMultipleDecoder = new PixxelPacketMultipleDecoder(vmp.maxPacketLength, p -> handlePacket(p)); packetDecoder = new PacketDecoder(vmp.maxPacketLength, p -> handlePacket(p)); packetDecoder.stripEncapsulationHeader(vmp.stripEncapsulationHeader); @@ -97,7 +100,7 @@ public void handle(DownlinkTransferFrame frame) { int dataEnd = frame.getDataEnd(); byte[] data = frame.getData(); - if (!vmp.usePixxelDecoder) { + if (vmp.tmDecoder == TMDecoder.CCSDS) { // Multiple packets from frame | With Segmentation try { int frameLoss = frame.lostFramesCount(lastFrameSeq); lastFrameSeq = frame.getVcFrameSeq(); @@ -127,8 +130,8 @@ public void handle(DownlinkTransferFrame frame) { eventProducer.sendWarning(e.toString()); } - } else { - try { + } else if (vmp.tmDecoder == TMDecoder.SINGLE) { // Single packet per frame | No Segmentation + try { int frameLoss = frame.lostFramesCount(lastFrameSeq); lastFrameSeq = frame.getVcFrameSeq(); @@ -152,6 +155,31 @@ public void handle(DownlinkTransferFrame frame) { ); eventProducer.sendWarning(e.toString()); } + } else { // Multiple packets per frame | No segmentation + try { + int frameLoss = frame.lostFramesCount(lastFrameSeq); + lastFrameSeq = frame.getVcFrameSeq(); + + if (frameLoss != 0) { + log.warn("Frame has been dropped, sigh"); + } + + if (packetStart != -1) { + pMultipleDecoder.process(data, packetStart, dataEnd - packetStart); + pMultipleDecoder.reset(); + } + } catch (TcTmException e) { + pMultipleDecoder.reset(); + eventProducer.sendWarning(e.toString()); + } catch (ArrayIndexOutOfBoundsException e) { + pMultipleDecoder.reset(); + log.warn(e.toString() + "\n" + + " Full Frame: " + StringConverter.arrayToHexString(data, true) + "\n" + + " Packet Start: " + packetStart + "\n" + + " Data (i.e Frame) End: " + dataEnd + "\n" + ); + eventProducer.sendWarning(e.toString()); + } } }