Added support for escaping byte sequences

1 files changed, 75 insertions, 32 deletions

diff --git a/src/main/java/org/traccar/protocol/G1rusProtocolDecoder.java b/src/main/java/org/traccar/protocol/G1rusProtocolDecoder.java
index 89303d1b1..d962e601e 100644
--- a/src/main/java/org/traccar/protocol/G1rusProtocolDecoder.java
+++ b/src/main/java/org/traccar/protocol/G1rusProtocolDecoder.java
@@ -3,6 +3,7 @@ package org.traccar.protocol;
 import com.google.common.primitives.Doubles;
 import com.google.common.primitives.Ints;
 import com.google.common.primitives.Longs;
+import com.google.common.primitives.Shorts;
 import io.netty.buffer.ByteBuf;
 import io.netty.channel.Channel;
 import org.slf4j.Logger;
@@ -63,6 +64,50 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
     private static final int G1RUS_GPS_VDOP_MASK = 0b01000000;
     private static final int G1RUS_GPS_STAT_MASK = 0b10000000;
 
+    private static final int G1RUS_ESCAPE_CHAR = 0x1B;
+
+
+    private byte readUnsignedByteUnescaped(ByteBuf buf) {
+        byte first = (byte) buf.readUnsignedByte();
+        if (first != G1RUS_ESCAPE_CHAR) {
+            return first;
+        } else { /* first == 0x1B */
+            byte second = (byte) buf.readUnsignedByte();
+            if (second == 0x00) {
+                return first;
+            } else { /* second == 0xE3 */
+                return (byte) 0xF8;
+            }
+        }
+    }
+
+
+    private void readBytesUnescaped(ByteBuf buf, byte[] to) {
+        for (int i = 0; i < to.length; ++i) {
+            to[i] = readUnsignedByteUnescaped(buf);
+        }
+    }
+
+    private void readBytesUnescaped(ByteBuf buf, byte[] to, int dstIndex, int length) {
+        for (int i = dstIndex; i < length; ++i) {
+            to[i] = readUnsignedByteUnescaped(buf);
+        }
+    }
+
+
+    private short readUnsignedShortUnescaped(ByteBuf buf) {
+        byte[] shortBuf = new byte[2];
+        readBytesUnescaped(buf, shortBuf);
+        return Shorts.fromByteArray(shortBuf);
+    }
+
+
+    private int readIntUnescaped(ByteBuf buf) {
+        byte[] intBuf = new byte[4];
+        readBytesUnescaped(buf, intBuf);
+        return Ints.fromByteArray(intBuf);
+    }
+
 
     private void decodeSYSSub(ByteBuf buf) {
         LOGGER.info("<SYS>");
@@ -75,23 +120,23 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
          */
 
         /* Device name */
-        byte devNameLen = (byte) buf.readUnsignedByte();
+        byte devNameLen = readUnsignedByteUnescaped(buf);
         byte[] devName = new byte[devNameLen & 0xF];
-        buf.readBytes(devName);
+        readBytesUnescaped(buf, devName);
         String devNameString = new String(devName);
         LOGGER.info("Device name: " + devNameString);
 
         /* Firmware version */
-        byte firmwareLen = (byte) buf.readUnsignedByte();
+        byte firmwareLen = readUnsignedByteUnescaped(buf);
         byte[] firmware = new byte[firmwareLen & 0xF];
-        buf.readBytes(firmware);
+        readBytesUnescaped(buf, firmware);
         String firmwareString = new String(firmware);
         LOGGER.info("Firmware version: " + firmwareString);
 
         /* Hardware version */
-        byte hardwareLen = (byte) buf.readUnsignedByte();
+        byte hardwareLen = readUnsignedByteUnescaped(buf);
         byte[] hardware = new byte[hardwareLen & 0xF];
-        buf.readBytes(hardware);
+        readBytesUnescaped(buf, hardware);
         String hardwareString = new String(hardware);
         LOGGER.info("Hardware version: " + hardwareString);
 
@@ -104,30 +149,30 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
 
         buf.skipBytes(1); /* Total length */
 
-        short subMask = (short) buf.readUnsignedShort();
+        short subMask = readUnsignedShortUnescaped(buf);
         if ((subMask & G1RUS_GPS_SIGN_MASK) == G1RUS_GPS_SIGN_MASK) {
             buf.skipBytes(1);
         }
         if ((subMask & G1RUS_GPS_POS_MASK) == G1RUS_GPS_POS_MASK) {
             byte[] pos_buf = new byte[4];
-            buf.readBytes(pos_buf);
+            readBytesUnescaped(buf, pos_buf);
             position.setLatitude((float) Ints.fromByteArray(pos_buf) / 1000000);
             LOGGER.info("Latitude: " + position.getLatitude());
 
-            buf.readBytes(pos_buf);
+            readBytesUnescaped(buf, pos_buf);
             position.setLongitude((float) Ints.fromByteArray(pos_buf) / 1000000);
             LOGGER.info("Longitude: " + position.getLongitude());
         }
         if ((subMask & G1RUS_GPS_SPD_MASK) == G1RUS_GPS_SPD_MASK) {
-            position.setSpeed(buf.readUnsignedShort());
+            position.setSpeed(readUnsignedShortUnescaped(buf));
             LOGGER.info("Speed: " + position.getSpeed());
         }
         if ((subMask & G1RUS_GPS_AZTH_MASK) == G1RUS_GPS_AZTH_MASK) {
-            position.setCourse(buf.readUnsignedShort());
+            position.setCourse(readUnsignedShortUnescaped(buf));
             LOGGER.info("Course: " + position.getCourse());
         }
         if ((subMask & G1RUS_GPS_ALT_MASK) == G1RUS_GPS_ALT_MASK) {
-            position.setAltitude(buf.readUnsignedShort());
+            position.setAltitude(readUnsignedShortUnescaped(buf));
             LOGGER.info("Altitude: " + position.getAltitude());
         }
         if ((subMask & G1RUS_GPS_HDOP_MASK) == G1RUS_GPS_HDOP_MASK) {
@@ -147,7 +192,7 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
 
 
     private Position decodeRegular(Channel channel, SocketAddress remoteAddress, ByteBuf buf, long imei, byte packetType) {
-        int timestamp_ = buf.readInt();
+        int timestamp_ = readIntUnescaped(buf);
         long timestamp = (946684800 + timestamp_) * 1000L; /* Convert received time to proper UNIX timestamp */
         LOGGER.info("Date and time: " + new SimpleDateFormat("yyyy.MM.dd HH:mm:ss").format(new Date(timestamp)));
 
@@ -158,7 +203,7 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
         DeviceSession deviceSession = null;
         Position position = null;
 
-        short dataUploadingMask = (short) buf.readUnsignedShort();
+        short dataUploadingMask = readUnsignedShortUnescaped(buf);
         if ((dataUploadingMask & G1RUS_DATA_SYS_MASK) == G1RUS_DATA_SYS_MASK) {
             decodeSYSSub(buf);
         }
@@ -174,24 +219,24 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
             decodeGPSSub(buf, position);
         }
         if ((dataUploadingMask & G1RUS_DATA_GSM_MASK) == G1RUS_DATA_GSM_MASK) {
-            buf.skipBytes(buf.readUnsignedByte());
+            buf.skipBytes(readUnsignedByteUnescaped(buf));
         }
         if ((dataUploadingMask & G1RUS_DATA_COT_MASK) == G1RUS_DATA_COT_MASK) {
-            buf.skipBytes(buf.readUnsignedByte());
+            buf.skipBytes(readUnsignedByteUnescaped(buf));
         }
         if ((dataUploadingMask & G1RUS_DATA_ADC_MASK) == G1RUS_DATA_ADC_MASK) {
             /*if (deviceSession == null) {
                 return null;
             }*/
 
-            buf.skipBytes(buf.readUnsignedByte());
+            buf.skipBytes(readUnsignedByteUnescaped(buf));
             // decodeADCSub(buf, position);
         }
         if ((dataUploadingMask & G1RUS_DATA_DTT_MASK) == G1RUS_DATA_DTT_MASK) {
-            buf.skipBytes(buf.readUnsignedByte());
+            buf.skipBytes(readUnsignedByteUnescaped(buf));
         }
         if ((dataUploadingMask & G1RUS_DATA_ETD_MASK) == G1RUS_DATA_ETD_MASK) {
-            buf.skipBytes(buf.readUnsignedByte());
+            buf.skipBytes(readUnsignedByteUnescaped(buf));
         }
 
         return position;
@@ -220,15 +265,17 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
     protected Object decode(Channel channel, SocketAddress remoteAddress, Object msg) throws Exception {
         ByteBuf buf = (ByteBuf) msg;
 
-        buf.skipBytes(1); /* Head */
+        if (buf.readUnsignedByte() != G1RUS_HEAD_TAIL) {
+            return null;
+        }
 
-        LOGGER.info("Protocol version: " + buf.readUnsignedByte());
+        LOGGER.info("Protocol version: " + readUnsignedByteUnescaped(buf));
 
-        byte packetType = (byte) buf.readUnsignedByte();
+        byte packetType = readUnsignedByteUnescaped(buf);
         printPacketType(packetType);
 
         byte[] imei = new byte[8];
-        buf.readBytes(imei, 0, 7);
+        readBytesUnescaped(buf, imei, 0, 7);
         long imei_long = Longs.fromByteArray(imei);
         LOGGER.info("IMEI: " + imei_long);
 
@@ -249,22 +296,18 @@ public class G1rusProtocolDecoder extends BaseProtocolDecoder {
         } else if ((packetType & G1RUS_TYPE_BCD_MASK) == G1RUS_TYPE_MIXED) {
             positions = new LinkedList<>();
 
-            while (buf.readableBytes() > 3) {
-                short subPacketLength = (short) buf.readUnsignedShort();
-                byte subPacketType = (byte) buf.readUnsignedByte();
+            while (buf.readableBytes() > 5) {
+                short subPacketLength = readUnsignedShortUnescaped(buf);
+                byte subPacketType = readUnsignedByteUnescaped(buf);
                 printPacketType(subPacketType);
 
                 if ((subPacketType & G1RUS_TYPE_BCD_MASK) == G1RUS_TYPE_REGULAR) {
-                    Position position = decodeRegular(channel, remoteAddress, buf, imei_long, packetType);
+                    Position position = decodeRegular(channel, remoteAddress, buf, imei_long, subPacketType);
                     if (position != null) {
                         positions.add(position);
                     }
                 } else {
-                    try {
-                        buf.skipBytes(subPacketLength - 1);
-                    } catch (Exception e) {
-                        return positions;
-                    }
+                    buf.skipBytes(subPacketLength - 1);
                 }
                 /* else if ((subPacketType & G1RUS_TYPE_BCD_MASK) == G1RUS_TYPE_SMS_FORWARD) {
                     buf.skipBytes(subPacketLength - 1);