Added strict implementation for frame decoder

3 files changed, 82 insertions, 27 deletions

diff --git a/src/org/traccar/protocol/NavisFrameDecoder.java b/src/org/traccar/protocol/NavisFrameDecoder.java
index 2480629ac..aa057b51f 100644
--- a/src/org/traccar/protocol/NavisFrameDecoder.java
+++ b/src/org/traccar/protocol/NavisFrameDecoder.java
@@ -25,17 +25,13 @@ public class NavisFrameDecoder extends BaseFrameDecoder {
 
     private static final int NTCB_HEADER_LENGHT = 16;
     private static final int NTCB_LENGHT_OFFSET = 12;
+    private static final int FLEX_HEADER_LENGHT = 2;
     private static final int MAX_FRAME_LENGHT = 65551;
 
-    private static byte src8Checksum(ByteBuf buf, int length) {
-        byte sum = (byte) 0xFF;
-        for (int i = 0; i < length; i++) {
-            sum ^= buf.getUnsignedByte(i);
-            for (int j = 0; j < 8; j++) {
-                sum = (sum & 0x80) != 0 ? (byte) ((sum << 1) ^ 0x31) : (byte) (sum << 1);
-            }
-        }
-        return sum;
+    private final NavisProtocolDecoder protocolDecoder;
+
+    public NavisFrameDecoder(NavisProtocolDecoder protocolDecoder) {
+        this.protocolDecoder = protocolDecoder;
     }
 
     @Override
@@ -52,12 +48,49 @@ public class NavisFrameDecoder extends BaseFrameDecoder {
             return buf.readRetainedSlice(1);
         } else if (buf.getByte(buf.readerIndex()) == 0x7E /* "~" */) {
             // FLEX frame
+            if (protocolDecoder.getFLEXDataSize() == 0) {
+                return null;
+            }
             if (buf.readableBytes() > MAX_FRAME_LENGHT) {
                 throw new TooLongFrameException();
             }
 
-            if (src8Checksum(buf, buf.readableBytes() - 1) == buf.getByte(buf.readableBytes() - 1)) {
-                return buf.readRetainedSlice(buf.readableBytes());
+            if (buf.readableBytes() > FLEX_HEADER_LENGHT) {
+                int length = 0;
+                switch (buf.getByte(buf.readerIndex() + 1)) {
+                    // FLEX 1.0
+                    case 0x41:  // "A"
+                        length = protocolDecoder.getFLEXDataSize()
+                                * buf.getByte(buf.readerIndex() + FLEX_HEADER_LENGHT) + 2;
+                        break;
+                    case 0x54:  // "T"
+                        length = protocolDecoder.getFLEXDataSize() + 5;
+                        break;
+                    case 0x43:  // "C"
+                        length = protocolDecoder.getFLEXDataSize() + 1;
+                        break;
+                    // FLEX 2.0 (Extra packages)
+                    case 0x45:  // "E"
+                        length++;
+                        for (int i = 0; i < buf.getByte(buf.readerIndex() + FLEX_HEADER_LENGHT); i++) {
+                            if (buf.readableBytes() > FLEX_HEADER_LENGHT + length + 1) {
+                                length += buf.getUnsignedShort(length + FLEX_HEADER_LENGHT) + 2;
+                            } else {
+                                return null;
+                            }
+                        }
+                        length++;
+                        break;
+                    case 0x58:  // "X"
+                        length = buf.getUnsignedShortLE(buf.readerIndex() + FLEX_HEADER_LENGHT) + 7;
+                        break;
+                    default:
+                        break;
+                }
+
+                if (buf.readableBytes() >= FLEX_HEADER_LENGHT + length) {
+                    return buf.readRetainedSlice(buf.readableBytes());
+                }
             }
         } else {
             // NTCB frame
diff --git a/src/org/traccar/protocol/NavisProtocol.java b/src/org/traccar/protocol/NavisProtocol.java
index 4250ba4dc..628b78fd4 100644
--- a/src/org/traccar/protocol/NavisProtocol.java
+++ b/src/org/traccar/protocol/NavisProtocol.java
@@ -25,10 +25,10 @@ public class NavisProtocol extends BaseProtocol {
         addServer(new TrackerServer(false, getName()) {
             @Override
             protected void addProtocolHandlers(PipelineBuilder pipeline) {
-                pipeline.addLast(new NavisFrameDecoder());
-                pipeline.addLast(new NavisProtocolDecoder(NavisProtocol.this));
+                NavisProtocolDecoder protocolDecoder = new NavisProtocolDecoder(NavisProtocol.this);
+                pipeline.addLast(new NavisFrameDecoder(protocolDecoder));
+                pipeline.addLast(protocolDecoder);
             }
         });
     }
-
 }
diff --git a/src/org/traccar/protocol/NavisProtocolDecoder.java b/src/org/traccar/protocol/NavisProtocolDecoder.java
index 3c5abc2a4..71dd1bcc7 100644
--- a/src/org/traccar/protocol/NavisProtocolDecoder.java
+++ b/src/org/traccar/protocol/NavisProtocolDecoder.java
@@ -35,11 +35,15 @@ import java.util.LinkedList;
 import java.util.List;
 import java.util.Date;
 
+// DEBUG:
+import org.traccar.helper.DataConverter;
+
 public class NavisProtocolDecoder extends BaseProtocolDecoder {
 
     private String prefix;
     private long deviceUniqueId, serverId;
-    private int flexDataSize;  // bits
+    private int flexDataSize;  // bytes
+    private int flexBitfieldDataSize;  // bits
     private final byte[] flexBitfield;
     private byte flexProtocolVersion, flexStructVersion;
     private static final Logger LOGGER = LoggerFactory.getLogger(NavisProtocolDecoder.class);
@@ -311,14 +315,15 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
     }
 
     private ParseResult parseFLEXPosition(DeviceSession deviceSession, ByteBuf buf) {
-        LOGGER.info("DEBUG: id={} parseFLEXPosition_START", deviceSession.getDeviceId());
+        LOGGER.info("DEBUG: id={} parseFLEXPosition_START, flexDataSize={}, flexBitfield[]={}",
+                deviceSession.getDeviceId(), flexBitfieldDataSize, DataConverter.printHex(flexBitfield));
         Position position = new Position(getProtocolName());
 
         position.setDeviceId(deviceSession.getDeviceId());
 
         long index = 0;
 
-        for (int i = 0; i < flexDataSize; i++) {
+        for (int i = 0; i < flexBitfieldDataSize; i++) {
             if ((flexBitfield[(int) (i / 8)] & (0x80 >> i % 8)) == 0) {
                 // Skip FLEX data field
                 continue;
@@ -505,7 +510,7 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
                     buf.skipBytes(2);
                     break;
                 case 68:  // CAN Speed
-                    position.set("can-speed", buf.readByte());
+                    position.set("can-speed", buf.readUnsignedByte());
                     break;
                 default:
                     break;
@@ -535,7 +540,7 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
     private Object processFLEXArray(DeviceSession deviceSession, Channel channel, ByteBuf buf) {
         List<Position> positions = new LinkedList<>();
         int count = buf.readUnsignedByte();
-        LOGGER.info("DEBUG: id={} processFLEXArray_COUNT={}", deviceSession.getDeviceId(), count);
+
         for (int i = 0; i < count; i++) {
             Position position = parseFLEXPosition(deviceSession, buf).getPosition();
             if (position.getFixTime() != null) {
@@ -556,12 +561,18 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
     }
 
     private Object processFLEXNegotiation(Channel channel, ByteBuf buf) {
-        if (buf.readByte() != (byte) 0xB0) {
+        int[] fieldsSizes = {4, 2, 4, 1, 1, 1, 1, 1, 4, 4, 4, 4, 4, 2, 4, 4,
+                             2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1,
+                             4, 4, 2, 2, 4, 2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1,
+                             1, 1, 1, 1, 2, 4, 2, 1, 4, 2, 2, 2, 2, 2, 1, 1,
+                             1, 2, 4, 2, 1};
+
+        if ((byte) buf.readUnsignedByte() != (byte) 0xB0) {
             return null;
         }
 
-        flexProtocolVersion = buf.readByte();
-        flexStructVersion = buf.readByte();
+        flexProtocolVersion = (byte) buf.readUnsignedByte();
+        flexStructVersion = (byte) buf.readUnsignedByte();
         if (flexProtocolVersion != (byte) 0x0A && flexProtocolVersion != (byte) 0x14) {
             return null;
         }
@@ -569,11 +580,18 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
             return null;
         }
 
-        flexDataSize = buf.readByte();
-        if (flexDataSize > 122) {
+        flexBitfieldDataSize = buf.readUnsignedByte();
+        if (flexBitfieldDataSize > 122) {
             return null;
         }
-        buf.readBytes(flexBitfield, 0, (int) Math.ceil(flexDataSize / 8));
+        buf.readBytes(flexBitfield, 0, (int) Math.ceil((double) flexBitfieldDataSize / 8));
+
+        flexDataSize = 0;
+        for (int i = 0; i < flexBitfieldDataSize; i++) {
+            if ((flexBitfield[(int) (i / 8)] & (0x80 >> i % 8)) != 0) {
+                flexDataSize += fieldsSizes[i];
+            }
+        }
 
         ByteBuf response = Unpooled.buffer(9);
         response.writeCharSequence("*<FLEX", StandardCharsets.US_ASCII);
@@ -586,7 +604,7 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
     }
 
     private Object processHandshake(Channel channel, SocketAddress remoteAddress, ByteBuf buf) {
-        buf.readByte();  // Colon symbol
+        buf.skipBytes(1);  // Colon symbol
         if (getDeviceSession(channel, remoteAddress, buf.toString(StandardCharsets.US_ASCII)) != null) {
             sendNTCBReply(channel, Unpooled.copiedBuffer("*<S", StandardCharsets.US_ASCII));
         }
@@ -656,7 +674,7 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
                         case 0x547E:  // "~T"
                         case 0x437E:  // "~C"
                             return processFLEXSingle(deviceSession, channel, buf);
-                        // FLEX 2.0
+                        // FLEX 2.0 (Extra packages)
                         case 0x457E:  // "~E"
                             LOGGER.info("DEBUG: id={} FLEX \"~E\"", deviceSession.getDeviceId());
                             break;
@@ -711,4 +729,8 @@ public class NavisProtocolDecoder extends BaseProtocolDecoder {
 
         return null;
     }
+
+    public int getFLEXDataSize() {
+        return flexDataSize;
+    }
 }