Merge branch 'master' of https://github.com/tananaev/traccar

1 files changed, 38 insertions, 3 deletions

diff --git a/src/org/traccar/protocol/CastelProtocolDecoder.java b/src/org/traccar/protocol/CastelProtocolDecoder.java
index e36d086f8..3179cbd78 100644
--- a/src/org/traccar/protocol/CastelProtocolDecoder.java
+++ b/src/org/traccar/protocol/CastelProtocolDecoder.java
@@ -115,9 +115,14 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
         ChannelBuffer buf = (ChannelBuffer) msg;
 
-        buf.skipBytes(2); // header
+        int header = buf.readUnsignedShort();
         buf.readUnsignedShort(); // length
-        int version = buf.readUnsignedByte();
+
+        int version = -1;
+        if (header == 0x4040) {
+            version = buf.readUnsignedByte();
+        }
+
         ChannelBuffer id = buf.readBytes(20);
         int type = ChannelBuffers.swapShort(buf.readShort());
 
@@ -125,7 +130,37 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
             return null;
         }
 
-        if (version == 4) {
+        if (version == -1) {
+
+            if (type == 0x2001) {
+
+                if (channel != null) {
+                    int length = 2 + 2 + id.readableBytes() + 2 + 4 + 8 + 2 + 2;
+
+                    ChannelBuffer response = ChannelBuffers.directBuffer(ByteOrder.LITTLE_ENDIAN, length);
+                    response.writeByte('@'); response.writeByte('@');
+                    response.writeShort(length);
+                    response.writeBytes(id);
+                    response.writeShort(ChannelBuffers.swapShort((short) 0x1001));
+                    response.writeInt(0);
+                    for (int i = 0; i < 8; i++) {
+                        response.writeByte(0xff);
+                    }
+                    response.writeShort(
+                            Checksum.crc16(Checksum.CRC16_X25, response.toByteBuffer(0, response.writerIndex())));
+                    response.writeByte(0x0D); response.writeByte(0x0A);
+                    channel.write(response, remoteAddress);
+                }
+
+                buf.readUnsignedInt(); // index
+                buf.readUnsignedInt(); // unix time
+                buf.readUnsignedByte();
+
+                return readPosition(buf);
+
+            }
+
+        } else if (version == 4) {
 
             if (type == MSG_SC_HEARTBEAT) {