Implement KHD protocol

7 files changed, 242 insertions, 20 deletions

diff --git a/default.cfg b/default.cfg
index a583a8f8b..2232f1098 100644
--- a/default.cfg
+++ b/default.cfg
@@ -294,4 +294,8 @@
     <entry key='taip.enable'>true</entry>
     <entry key='taip.port'>5057</entry>
 
+    <!-- KHD server configuration -->
+    <entry key='khd.enable'>true</entry>
+    <entry key='khd.port'>5058</entry>
+
 </properties>
diff --git a/src/org/traccar/ServerManager.java b/src/org/traccar/ServerManager.java
index cd8a670ce..8e2465a2b 100644
--- a/src/org/traccar/ServerManager.java
+++ b/src/org/traccar/ServerManager.java
@@ -159,6 +159,7 @@ public class ServerManager {
         initOsmAndServer("osmand");
         initEasyTrackServer("easytrack");
         initTaipServer("taip");
+        initKhdServer("khd");
         
         // Initialize web server
         if (Boolean.valueOf(properties.getProperty("http.enable"))) {
@@ -1031,4 +1032,16 @@ public class ServerManager {
         }
     }
 
+    private void initKhdServer(String protocol) throws SQLException {
+        if (isProtocolEnabled(properties, protocol)) {
+            serverList.add(new TrackerServer(this, new ServerBootstrap(), protocol) {
+                @Override
+                protected void addSpecificHandlers(ChannelPipeline pipeline) {
+                    pipeline.addLast("frameDecoder", new LengthFieldBasedFrameDecoder(256, 3, 2));
+                    pipeline.addLast("objectDecoder", new KhdProtocolDecoder(ServerManager.this));
+                }
+            });
+        }
+    }
+
 }
diff --git a/src/org/traccar/helper/ChannelBufferTools.java b/src/org/traccar/helper/ChannelBufferTools.java
index c0df86bcd..ef2ebc313 100644
--- a/src/org/traccar/helper/ChannelBufferTools.java
+++ b/src/org/traccar/helper/ChannelBufferTools.java
@@ -97,6 +97,28 @@ public class ChannelBufferTools {
         
         return result.toString();
     }
+    
+    /**
+     * Read BCD coded coordinate (first byte has sign bit)
+     */
+    public static double readCoordinate(ChannelBuffer buf) {
+        int b1 = buf.readUnsignedByte();
+        int b2 = buf.readUnsignedByte();
+        int b3 = buf.readUnsignedByte();
+        int b4 = buf.readUnsignedByte();
+        
+        double value = (b2 & 0xf) * 10 + (b3 >> 4);
+        value += (((b3 & 0xf) * 10 + (b4 >> 4)) * 10 + (b4 & 0xf)) / 1000.0;
+        value /= 60;
+        value += ((b1 >> 4 & 0x7) * 10 + (b1 & 0xf)) * 10 + (b2 >> 4);
+        
+        if ((b1 & 0x80) != 0) {
+            value = -value;
+        }
+        
+        return value;
+    }
+
 
     /**
      * Convert integer array to byte array
diff --git a/src/org/traccar/helper/Crc.java b/src/org/traccar/helper/Crc.java
index 84224a781..5a668f30a 100644
--- a/src/org/traccar/helper/Crc.java
+++ b/src/org/traccar/helper/Crc.java
@@ -136,6 +136,14 @@ public class Crc {
         }
         return (int) checksum.getValue();
     }
+
+    public static int xorChecksum(ByteBuffer buf) {
+        int checksum = 0;
+        for (int i = 0; i < buf.remaining(); i++) {
+            checksum ^= buf.get(i);
+        }
+        return checksum;
+    }
     
     public static String nmeaChecksum(String msg) {
         int checksum = 0;
diff --git a/src/org/traccar/protocol/GatorProtocolDecoder.java b/src/org/traccar/protocol/GatorProtocolDecoder.java
index 041e44588..b2baf455a 100644
--- a/src/org/traccar/protocol/GatorProtocolDecoder.java
+++ b/src/org/traccar/protocol/GatorProtocolDecoder.java
@@ -45,24 +45,6 @@ public class GatorProtocolDecoder extends BaseProtocolDecoder {
     private static final int PACKET_CAMERA_RESPONSE = 0x56;
     private static final int PACKET_PICTURE_DATA = 0x57;
     
-    private double readCoordinate(ChannelBuffer buf) {
-        int b1 = buf.readUnsignedByte();
-        int b2 = buf.readUnsignedByte();
-        int b3 = buf.readUnsignedByte();
-        int b4 = buf.readUnsignedByte();
-        
-        double value = (b2 & 0xf) * 10 + (b3 >> 4);
-        value += (((b3 & 0xf) * 10 + (b4 >> 4)) * 10 + (b4 & 0xf)) / 1000.0;
-        value /= 60;
-        value += ((b1 >> 4 & 0x7) * 10 + (b1 & 0xf)) * 10 + (b2 >> 4);
-        
-        if ((b1 & 0x80) != 0) {
-            value = -value;
-        }
-        
-        return value;
-    }
-
     @Override
     protected Object decode(
             ChannelHandlerContext ctx, Channel channel, Object msg)
@@ -105,8 +87,8 @@ public class GatorProtocolDecoder extends BaseProtocolDecoder {
             position.setTime(time.getTime());
 
             // Location
-            position.setLatitude(readCoordinate(buf));
-            position.setLongitude(readCoordinate(buf));
+            position.setLatitude(ChannelBufferTools.readCoordinate(buf));
+            position.setLongitude(ChannelBufferTools.readCoordinate(buf));
             position.setSpeed(ChannelBufferTools.readHexInteger(buf, 4) * 0.539957);
             position.setCourse((double) ChannelBufferTools.readHexInteger(buf, 4));
             position.setAltitude(0.0);
diff --git a/src/org/traccar/protocol/KhdProtocolDecoder.java b/src/org/traccar/protocol/KhdProtocolDecoder.java
new file mode 100644
index 000000000..04317c442
--- /dev/null
+++ b/src/org/traccar/protocol/KhdProtocolDecoder.java
@@ -0,0 +1,152 @@
+/*
+ * Copyright 2014 Anton Tananaev (anton.tananaev@gmail.com)
+ *
+ * Licensed under the Apache License, Version 2.0 (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
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.traccar.protocol;
+
+import java.util.Calendar;
+import java.util.TimeZone;
+import org.jboss.netty.buffer.ChannelBuffer;
+import org.jboss.netty.buffer.ChannelBuffers;
+import org.jboss.netty.channel.Channel;
+import org.jboss.netty.channel.ChannelHandlerContext;
+import org.traccar.BaseProtocolDecoder;
+import org.traccar.ServerManager;
+import org.traccar.helper.ChannelBufferTools;
+import org.traccar.helper.Crc;
+import org.traccar.helper.Log;
+import org.traccar.model.ExtendedInfoFormatter;
+import org.traccar.model.Position;
+
+public class KhdProtocolDecoder extends BaseProtocolDecoder {
+
+    public KhdProtocolDecoder(ServerManager serverManager) {
+        super(serverManager);
+    }
+
+    private String readSerialNumber(ChannelBuffer buf) {
+        int b1 = buf.readUnsignedByte();
+        int b2 = buf.readUnsignedByte(); if (b2 > 0x80) b2 -= 0x80;
+        int b3 = buf.readUnsignedByte(); if (b3 > 0x80) b3 -= 0x80;
+        int b4 = buf.readUnsignedByte();
+        String serialNumber = String.format("%02d%02d%02d%02d", b1, b2, b3, b4);
+        return String.valueOf(Integer.valueOf(serialNumber));
+    }
+
+    private static final int MSG_LOGIN = 0xB1;
+    private static final int MSG_CONFIRMATION = 0x21;
+    private static final int MSG_ON_DEMAND = 0x81;
+    private static final int MSG_POSITION_UPLOAD = 0x80;
+    private static final int MSG_POSITION_REUPLOAD = 0x8E;
+    private static final int MSG_ALARM = 0x82;
+    private static final int MSG_REPLY = 0x85;
+    private static final int MSG_PERIPHERAL = 0xA3;
+
+    @Override
+    protected Object decode(
+            ChannelHandlerContext ctx, Channel channel, Object msg)
+            throws Exception {
+
+        ChannelBuffer buf = (ChannelBuffer) msg;
+
+        buf.skipBytes(2); // header
+        int type = buf.readUnsignedByte();
+        buf.readUnsignedShort(); // size
+
+        if (type == MSG_ON_DEMAND ||
+            type == MSG_POSITION_UPLOAD ||
+            type == MSG_POSITION_REUPLOAD ||
+            type == MSG_ALARM ||
+            type == MSG_REPLY ||
+            type == MSG_PERIPHERAL) {
+
+            // Create new position
+            Position position = new Position();
+            ExtendedInfoFormatter extendedInfo = new ExtendedInfoFormatter("khd");
+
+            // Device identification
+            String id = readSerialNumber(buf);
+            try {
+                position.setDeviceId(getDataManager().getDeviceByImei(id).getId());
+            } catch(Exception error) {
+                Log.warning("Unknown device - " + id);
+            }
+            
+            // Date and time
+            Calendar time = Calendar.getInstance(TimeZone.getTimeZone("UTC"));
+            time.clear();
+            time.set(Calendar.YEAR, 2000 + ChannelBufferTools.readHexInteger(buf, 2));
+            time.set(Calendar.MONTH, ChannelBufferTools.readHexInteger(buf, 2) - 1);
+            time.set(Calendar.DAY_OF_MONTH, ChannelBufferTools.readHexInteger(buf, 2));
+            time.set(Calendar.HOUR, ChannelBufferTools.readHexInteger(buf, 2));
+            time.set(Calendar.MINUTE, ChannelBufferTools.readHexInteger(buf, 2));
+            time.set(Calendar.SECOND, ChannelBufferTools.readHexInteger(buf, 2));
+            position.setTime(time.getTime());
+
+            // Location
+            position.setLatitude(ChannelBufferTools.readCoordinate(buf));
+            position.setLongitude(ChannelBufferTools.readCoordinate(buf));
+            position.setSpeed(ChannelBufferTools.readHexInteger(buf, 4) * 0.539957);
+            position.setCourse((double) ChannelBufferTools.readHexInteger(buf, 4));
+            position.setAltitude(0.0);
+
+            // Flags
+            int flags = buf.readUnsignedByte();
+            position.setValid((flags & 0x80) != 0);
+            
+            if (type == MSG_ALARM) {
+                
+                buf.skipBytes(2);
+
+            } else {
+
+                // Milage
+                extendedInfo.set("milage", buf.readUnsignedMedium());
+            
+                // Status
+                buf.skipBytes(4);
+                
+                // Other
+                buf.skipBytes(8);
+
+            }
+            
+            // TODO: parse extra data
+
+            position.setExtendedInfo(extendedInfo.toString());
+            return position;
+        }
+
+        else if (type == MSG_LOGIN && channel != null) {
+            
+            buf.skipBytes(4); // serial number
+            buf.readByte(); // reserved
+            
+            ChannelBuffer response = ChannelBuffers.directBuffer(10);
+            response.writeByte(0x29); response.writeByte(0x29); // header
+            response.writeByte(MSG_CONFIRMATION);
+            response.writeShort(5); // size
+            response.writeByte(buf.readUnsignedByte());
+            response.writeByte(type);
+            response.writeByte(0); // reserved
+            response.writeByte(Crc.xorChecksum(response.toByteBuffer(0, 8)));
+            response.writeByte(0x0D); // ending
+            channel.write(response);
+
+        }
+        
+        return null;
+    }
+
+}
diff --git a/test/org/traccar/protocol/KhdProtocolDecoderTest.java b/test/org/traccar/protocol/KhdProtocolDecoderTest.java
new file mode 100644
index 000000000..44d8ac2bd
--- /dev/null
+++ b/test/org/traccar/protocol/KhdProtocolDecoderTest.java
@@ -0,0 +1,41 @@
+package org.traccar.protocol;
+
+import org.traccar.helper.TestDataManager;
+import org.jboss.netty.buffer.ChannelBuffers;
+import static org.traccar.helper.DecoderVerifier.verify;
+import static org.junit.Assert.assertNull;
+import org.junit.Test;
+import org.traccar.helper.ChannelBufferTools;
+
+public class KhdProtocolDecoderTest {
+
+    @Test
+    public void testDecode() throws Exception {
+
+        KhdProtocolDecoder decoder = new KhdProtocolDecoder(null);
+        decoder.setDataManager(new TestDataManager());
+
+        int[] buf1 = {0x29,0x29,0xb1,0x00,0x06,0x05,0x16,0x29,0x35,0xb8,0x0d};
+        assertNull(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf1))));
+
+        int[] buf2 = {0x29,0x29,0x80,0x00,0x28,0x05,0x16,0x29,0x35,0x14,0x01,0x08,0x07,0x47,0x27,0x80,0x11,0x29,0x67,0x03,0x65,0x33,0x69,0x00,0x00,0x01,0x03,0xff,0xff,0x00,0x00,0x82,0xfc,0x00,0x00,0x00,0x1e,0x78,0x09,0x1b,0x00,0x00,0x00,0x36,0x0d};
+        verify(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf2))));
+        
+        int[] buf3 = {0x29,0x29,0x81,0x00,0x28,0x0A,0x9F,0x95,0x38,0x08,0x12,0x28,0x16,0x01,0x31,0x02,0x23,0x94,0x30,0x11,0x40,0x37,0x25,0x00,0x00,0x03,0x30,0xFF,0x00,0x00,0x00,0x7F,0xFC,0x0F,0x00,0x00,0x1E,0x00,0x00,0x00,0x00,0x00,0x34,0x29,0x0D};
+        verify(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf3))));
+        
+        int[] buf4 = {0x29,0x29,0x80,0x00,0x28,0x0A,0x81,0x85,0x0A,0x12,0x03,0x10,0x09,0x57,0x50,0x00,0x52,0x81,0x37,0x00,0x61,0x19,0x08,0x00,0x00,0x02,0x32,0xF8,0x48,0xFF,0xBB,0xFF,0xFF,0x00,0x00,0x00,0x1E,0x00,0x00,0x00,0x00,0x00,0x00,0xED,0x0D};
+        verify(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf4))));
+        
+        int[] buf5 = {0x29,0x29,0x8E,0x00,0x28,0x0F,0x80,0x81,0x5A,0x12,0x12,0x18,0x20,0x31,0x16,0x02,0x23,0x18,0x46,0x11,0x40,0x22,0x70,0x00,0x72,0x02,0x62,0xFB,0x00,0x07,0x7C,0x7F,0xBF,0x56,0x00,0x00,0x1E,0x3C,0x32,0x00,0x00,0x00,0x00,0x85,0x0D};
+        verify(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf5))));
+        
+        int[] buf6 = {0x29,0x29,0x82,0x00,0x23,0x0A,0xA2,0xCC,0x39,0x12,0x05,0x03,0x05,0x05,0x22,0x02,0x85,0x94,0x79,0x03,0x10,0x95,0x50,0x00,0x80,0x02,0x07,0x84,0x00,0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x75,0x0D};
+        verify(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf6))));
+        
+        int[] buf7 = {0x29,0x29,0x85,0x00,0x08,0x1D,0xD0,0x8C,0x22,0x12,0x03,0x12,0x17,0x40,0x26,0x02,0x65,0x45,0x71,0x03,0x12,0x54,0x17,0x00,0x00,0x00,0x00,0xF8,0x19,0xC8,0x39,0xFF,0xFF,0x1D,0x00,0x00,0x1E,0x00,0x50,0x00,0x00,0x00,0x3A,0xF9,0x0D};
+        verify(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(ChannelBufferTools.convertArray(buf7))));
+
+    }
+
+}