using System; using System.Collections.Generic; using System.Linq; using System.Text; using SafeNetLib; using System.Net.Sockets; using System.Threading; using System.Net.NetworkInformation; namespace Teltonika_SOC { // --------------------------------------------------- // This class handles a client connection // --------------------------------------------------- class ConnectionThread { public TcpClient client; public static int connections = 0; public static int count = 0; // ------------------------------------------------------ public ConnectionThread() { Utils.ConsWrite(DebugMSG_Type.Debug, " Connection Thread created ...."); LOGS.LOG(DateTime.Now.ToString() + " : Connection Thread created ...."); } ~ConnectionThread() { } public void HandleConnection() { byte[] data = new byte[2048]; NetworkStream ns = client.GetStream(); ns.ReadTimeout = 60000; //Console.WriteLine("read timeout = " + ns.ReadTimeout); connections++; Utils.ConsWrite(DebugMSG_Type.Debug, "New client accepted: " + connections + " active connections"); LOGS.LOG(DateTime.Now.ToString() + " : New client accepted: " + connections + " active connections"); StringBuilder IMEIString = new StringBuilder(); //get IMEI byte[] myReadBuffer2 = new byte[2048]; //Int64 IMEI = 0; int numberOfBytesRead2 = 0; Thread.Sleep(1); try { do { numberOfBytesRead2 = ns.Read(myReadBuffer2, 0, myReadBuffer2.Length); IMEIString.AppendFormat("{0}", Encoding.ASCII.GetString(myReadBuffer2, 2, numberOfBytesRead2 - 2)); } while (ns.DataAvailable); Utils.ConsWrite(DebugMSG_Type.Debug, "Retrived imei:" + IMEIString); LOGS.LOG(DateTime.Now.ToString() + " : Retrived imei:" + IMEIString); byte[] response = { 0x01 }; ns.Write(response, 0, response.Length); ns.Flush(); } catch (Exception ee) { Utils.ConsWrite(DebugMSG_Type.always, "Error on Reading IMEI \n:" + ee.ToString()); LOGS.LOG(DateTime.Now.ToString() + " : Error on Reading IMEI \n:" + ee.ToString()); } //end of get IMEI while (true) { try { if (ns.CanRead) { byte[] myReadBuffer = new byte[2048]; StringBuilder myCompleteMessage = new StringBuilder(); int numberOfBytesRead = 0; Thread.Sleep(1); // check for new data available on the socket try { do { numberOfBytesRead = ns.Read(myReadBuffer, 0, myReadBuffer.Length); myCompleteMessage.AppendFormat("{0}", Encoding.ASCII.GetString(myReadBuffer, 0, numberOfBytesRead)); } while (ns.DataAvailable); if (myCompleteMessage.Length == 0) { break; } } catch (Exception ee) { //Console.WriteLine(ee.ToString()); break; } Utils.ConsWrite(DebugMSG_Type.GPS, "Received: (len=" + numberOfBytesRead + ")"); LOGS.LOG(DateTime.Now.ToString() + " : Received: (len=" + numberOfBytesRead + ")"); // process the message Byte toreturn = ProcessPacket2(myReadBuffer, numberOfBytesRead, IMEIString.ToString().Trim()); if (toreturn != 0) { byte[] response2 = { 0x00, 0x00, 0x00, 0x00 }; response2[3] = toreturn; ns.Write(response2, 0, response2.Length); ns.Flush(); #region setup mode if (Program.setupMode) { string imei = IMEIString.ToString(); if(Program.ImeiHT.Contains(imei)) { if ((int)Program.ImeiHT[imei] < Program.parametersCount) { string str = Program.commands[(int)Program.ImeiHT[imei]]; response2 = getByteCommand(str); ns.Write(response2, 0, response2.Length); ns.Flush(); Program.ImeiHT[imei] = (int)Program.ImeiHT[imei] + 1; Utils.WriteLine("Command <<" + str + ">> has been sent to imei " + IMEIString.ToString(), ConsoleColor.Yellow,Program.isUnix); if ((int)Program.ImeiHT[imei] == Program.parametersCount) { Utils.WriteLine("Setup finish for imei: " + IMEIString.ToString(), ConsoleColor.Yellow, Program.isUnix); } } } } #endregion setup mode } else { string response = getDeviceResponse(myReadBuffer); Utils.WriteLine("Response from unit " + IMEIString.ToString().Trim() + ": " + response, ConsoleColor.Yellow, Program.isUnix); //byte[] response2 = { 0x00, 0x00, 0x00, 0x00 }; //response2[3] = toreturn; //ns.Write(response2, 0, response2.Length); //ns.Flush(); } } else { Utils.ConsWrite(DebugMSG_Type.Debug, "Sorry. You cannot read from this NetworkStream."); LOGS.LOG(DateTime.Now.ToString() + " : Sorry. You cannot read from this NetworkStream."); break; } } catch (Exception ex) { Utils.ConsWrite(DebugMSG_Type.always, "HandleConnection ERRROR!!! \n: " + ex.ToString()); LOGS.LOG(DateTime.Now.ToString() + " : HandleConnection ERRROR!!! \n: " + ex.ToString()); break; } } ns.Flush(); ns.Close(); //client.Close(); Utils.CloseClient(client); connections--; Utils.ConsWrite(DebugMSG_Type.Debug, "Client disconnected: " + connections + " active connections"); LOGS.LOG(DateTime.Now.ToString() + " : Client disconnected: " + connections + " active connections"); } float ProcessGPSLat(String latitude, String lat_dir) { try { long LAT, LAT2; char LAT3; long llat, grade, zec; char[] param = new char[50]; String[] latList; float flat; param = lat_dir.ToCharArray(); if (param[0] != 'N' && param[0] != 'S') return 0; //Console.WriteLine("Latitude " + latitude + " dir " + lat_dir); latList = latitude.Split('.'); LAT = Convert.ToInt32(latList[0]); if (latList[1].Length > 4) latList[1] = latList[1].Remove(4); LAT2 = Convert.ToInt32(latList[1]); LAT3 = param[0]; // process the lat and lng for display grade = (LAT / 100L); zec = (LAT % 100L) * 1000L + LAT2 / 10; // get MMMMM*1000, from MM.mmmmm by MM*1000+mmm (0-59999) zec = (zec * 100L) / 60L; // translate MMMMM*1000 to DD * 100 * 1000 (0-99998) grade = grade * 100000 + zec; // translate all to DDddddd llat = grade; flat = (float)llat / 100000; if (param[0] == 'S') flat = -flat; if (flat < -90 || flat > 90) { Console.WriteLine("[warning \"overflow lat\": flat={0} llat={1}]", flat, llat); return 0; } return flat; } catch (Exception ee) { Utils.ConsWrite(DebugMSG_Type.always, "Error in ProcessGPSLat" + ee.ToString()); LOGS.LOG(DateTime.Now.ToString() + " : Error in ProcessGPSLat" + ee.ToString()); return 0; } } float ProcessGPSLong(String longitude, String lng_dir) { try { long LNG, LNG2; char LNG3; long llng, grade, zec; char[] param = new char[50]; String[] lngList; float flong; param = lng_dir.ToCharArray(); if (param[0] != 'E' && param[0] != 'W') return 0; lngList = longitude.Split('.'); LNG = Convert.ToInt32(lngList[0]); if (lngList[1].Length > 4) lngList[1] = lngList[1].Remove(4); LNG2 = Convert.ToInt32(lngList[1]); LNG3 = param[0]; grade = LNG / 100; // get DD (0-90) zec = (LNG % 100L) * 1000L + LNG2 / 10; // get MMMMM*1000, from MM.mmmmm by MM*1000+mmm (0-59999) zec = (zec * 100L) / 60L; // translate MMMMM*1000 to DD * 100 * 1000 (0-99998) grade = grade * 100000 + zec; // translate all to DDddddd llng = grade; flong = (float)llng / 100000; if (param[0] == 'W') flong = -flong; if (flong < -180 || flong > 180) { Console.WriteLine("[warning \"overflow lng\": flng={0} llng={1}]", flong, llng); return 0; } return flong; } catch (Exception ee) { Utils.ConsWrite(DebugMSG_Type.always, "Error in ProcessGPSLong" + ee.ToString()); LOGS.LOG(DateTime.Now.ToString() + " : Error in ProcessGPSLong" + ee.ToString()); return 0; } } #region old code // --------------------------------------------------- // Packet and command processing // --------------------------------------------------- public Byte ProcessPacket_beforeAdrian(byte[] data, int len, String imei) { String pLat = "0.0", pLong = "0.0"; Int64 itime70 = 0; Int32 iSpeed = 0; Int64 ilat, ilong; //first 4 bytes are 0x00,0x00, //second 4 bytes lenght i don't need on first view //code for GPS data 0x08 if (data[8] != 0x08) { return 0x00; } //number of message Int32 numberofMessage = 0; numberofMessage = data[9]; Byte numberofMessByte = data[9]; Int32 step = 10; for (int i = 0; i < numberofMessage; i++) { //itime70 = data[step]*256+data[step]*256 //get time //Console.WriteLine(); //Console.Write("Bytes for time:"); itime70 = data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); step++; for (int j = 0; j < 7; j++) { itime70 <<= 8; itime70 |= (Int64)data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); step++; } //jump priority 0 step++; //proccess LONG //Console.WriteLine(); //Console.Write("Bytes for LONG:"); bool sign = false; if ((data[step] & 0x80) != 0) { sign = true; } ilong = data[step]; Console.Write(" byte: 0x" + data[step].ToString("X2")); step++; for (int j = 0; j < 3; j++) { ilong <<= 8; ilong |= (Int64)data[step]; Console.Write(" 0x" + data[step].ToString("X2")); step++; } if (sign) { ilong = ilong - 0xFFFFFFFF; } double dlong = (Double)((Double)ilong / 10000000); pLong = Convert.ToString(dlong); //proccess LAT // Console.WriteLine(); // Console.Write("Bytes for LAT:"); sign = false; if ((data[step] & 0x80) != 0) { sign = true; } ilat = data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); step++; for (int j = 0; j < 3; j++) { ilat <<= 8; ilat |= (Int64)data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); step++; } if (sign) ilat = ilat - 0xFFFFFFFF; double dlat = (Double)((Double)ilat / 10000000); pLat = Convert.ToString(dlat); //jump altitude 2 bytes step += 2; //jump angle 2 bytes step += 2; //jump number of sattelites step++; //proccess speed // Console.WriteLine(); //Console.Write("Bytes for Speed:"); iSpeed = data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); iSpeed <<= 8; step++; iSpeed |= (Int32)data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); step++; int IOID = (Int32)data[step]; Console.WriteLine("IOID:" + IOID); step++; int IOrecords = (Int32)data[step]; Console.WriteLine("IOrecords:" + IOrecords); step++; int DImask =0; int NR1ByteIO = (Int32)data[step]; Console.WriteLine("NR1ByteIO:" + NR1ByteIO); step++; for (int k = 0; k < NR1ByteIO; k++) { Console.WriteLine("DI 1 byte: "); step += 2; //1 byte ID 1 byte value if (data[step + 1] == 1) DImask +=(int) Math.Pow(2, data[step]-1); } int NR2ByteIO = (Int32)data[step]; Console.WriteLine("NR2ByteIO:" + NR2ByteIO); step++; for (int k = 0; k < NR2ByteIO; k++) { Console.WriteLine("DI 2 byte: "); step += 3; //1 byte ID 2 byte value } int NR4ByteIO = (Int32)data[step]; Console.WriteLine("NR4ByteIO:" + NR4ByteIO); step++; for (int k = 0; k < NR2ByteIO; k++) { Console.WriteLine("DI 4 byte: "); step += 5; //1 byte ID 4 byte value } int NR8ByteIO = (Int32)data[step]; Console.WriteLine("NR8ByteIO:" + NR8ByteIO); step++; for (int k = 0; k < NR2ByteIO; k++) { Console.WriteLine("DI 8 byte: "); step += 9; //1 byte ID 8 byte value } //Console.WriteLine(); //Console.WriteLine("Time:"+itime70+" LONG:"+ilong+" LAT:"+ilat+" Speed:"+iSpeed); //add data to location QUEUE try { //locManager.SendLoc2messagebus(imei, (Int32)(itime70/1000), iSpeed.ToString(), pLat, pLong); htCell_t cell = new htCell_t(); cell.suid = imei; cell.spd = iSpeed.ToString(); cell.lat = pLat; cell.lng = pLong; cell.d_lat = dlat; cell.d_lng = dlong; cell.location_time = Utils.UnixTimeStampToDateTime(itime70 / 1000); Utils.WriteLine("GPS for SUID: " + imei + " with time: " + cell.location_time + " lat: " + pLat + " lng: " + pLong, ConsoleColor.Gray,Program.isUnix); SN_Queues.DBQueueLocation.PostItem(cell); Utils.ConsWrite(DebugMSG_Type.GPS, "Message added to queue!!!"); //process ALERTS if (SN_Queues.ht_SUInfo.ContainsKey(imei)) { Utils.ConsWrite(DebugMSG_Type.ALERTS, "Processing alerts for: "+imei); Utils.ConsWrite(DebugMSG_Type.ALERTS, "Message mask: " + DImask); SUinfo sui = (SUinfo)SN_Queues.ht_SUInfo[imei]; foreach (Alert alert in sui.alertList) { if (alert.Type == Alert_TYPE.DI) if (alert.DImask1 == DImask) { SN_Queues.alertQueue.PostItem(alert); Utils.ConsWrite(DebugMSG_Type.ALERTS, "Alert: " + alert.Alert_name + " added to queue"); } } } } catch (Exception ex) { Utils.ConsWrite(DebugMSG_Type.always, "ConnectionThread.cs =>> Error adding location to queue!!!"); Utils.ConsWrite(DebugMSG_Type.always, ex.ToString()); } } return numberofMessByte; } #endregion old code // --------------------------------------------------- // Packet and command processing // --------------------------------------------------- public Byte ProcessPacket2(byte[] data, int len, String imei) { String pLat = "0.0", pLong = "0.0"; Int64 itime70 = 0; Int32 iSpeed = 0; Int64 ilat, ilong; Int16 IOID; //first 4 bytes are 0x00,0x00, //second 4 bytes lenght i don't need on first view //code for GPS data 0x08 if (data[8] != 0x08) { return 0x00; } //number of message Int32 numberofMessage = 0; numberofMessage = data[9]; Byte numberofMessByte = data[9]; Int32 step = 10; for (int i = 0; i < numberofMessage; i++) { //itime70 = data[step]*256+data[step]*256 //get time //Console.WriteLine(); //Console.Write("Bytes for time:"); itime70 = data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); step++; for (int j = 0; j < 7; j++) { itime70 <<= 8; itime70 |= (Int64)data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); step++; } //jump priority 0 step++; //proccess LONG //Console.WriteLine(); //Console.Write("Bytes for LONG:"); bool sign = false; if ((data[step] & 0x80) != 0) { sign = true; } ilong = data[step]; //Console.Write(" byte: 0x" + data[step].ToString("X2")); step++; for (int j = 0; j < 3; j++) { ilong <<= 8; ilong |= (Int64)data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); step++; } if (sign) { ilong = ilong - 0xFFFFFFFF; } double dlong = (Double)((Double)ilong / 10000000); pLong = Convert.ToString(dlong); //proccess LAT // Console.WriteLine(); // Console.Write("Bytes for LAT:"); sign = false; if ((data[step] & 0x80) != 0) { sign = true; } ilat = data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); step++; for (int j = 0; j < 3; j++) { ilat <<= 8; ilat |= (Int64)data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); step++; } if (sign) ilat = ilat - 0xFFFFFFFF; double dlat = (Double)((Double)ilat / 10000000); pLat = Convert.ToString(dlat); //jump altitude 2 bytes step += 2; //jump angle 2 bytes step += 2; //jump number of sattelites step++; //proccess speed // Console.WriteLine(); //Console.Write("Bytes for Speed:"); #region SPEED // get MSB iSpeed = data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); // LEFT SHIFT == multiply with 2^8 iSpeed <<= 8; step++; // LOGIC OR iSpeed |= (Int32)data[step]; //Console.Write(" 0x" + data[step].ToString("X2")); #endregion step++; #region IOID List sourceTelemetry = new List(); IOID = (Int16)data[step++]; //Console.WriteLine("IOID:" + IOID); int IOrecords = (Int32)data[step]; //Console.WriteLine("IOrecords:" + IOrecords); step++; int DImask = 0; int NR1ByteIO = (Int32)data[step]; //Console.WriteLine("NR1ByteIO:" + NR1ByteIO); step++; for (int k = 0; k < NR1ByteIO; k++) { Telemetry tel = new Telemetry(); tel.ID = (Int32)data[step]; tel.TelemetryValue = (Int32)data[step + 1]; sourceTelemetry.Add(tel); //Console.WriteLine("DI 1 byte: "); if (data[step + 1] == 1) DImask += (int)Math.Pow(2, data[step]); step += 2; //1 byte ID 1 byte value } int NR2ByteIO = (Int32)data[step]; Console.WriteLine("NR2ByteIO:" + NR2ByteIO); step++; for (int k = 0; k < NR2ByteIO; k++) { Telemetry tel = new Telemetry(); tel.ID = (Int32)data[step]; step++; tel.TelemetryValue = data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); // LEFT SHIFT == multiply with 2^8 tel.TelemetryValue <<= 8; step++; // LOGIC OR tel.TelemetryValue |= (Int32)data[step + 2]; sourceTelemetry.Add(tel); //Console.WriteLine("DI 2 byte: "); step++; //1 byte ID 2 byte value } int NR4ByteIO = (Int32)data[step]; //Console.WriteLine("NR4ByteIO:" + NR4ByteIO); step++; for (int k = 0; k < NR4ByteIO; k++) { Telemetry tel = new Telemetry(); tel.ID = (Int32)data[step]; step++; for (int j = 0; j < 3; j++) { tel.TelemetryValue <<= 8; tel.TelemetryValue |= (Int32)data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); step++; } sourceTelemetry.Add(tel); //Console.WriteLine("DI 4 byte: "); //step += 5; //1 byte ID 4 byte value step++; } int NR8ByteIO = (Int32)data[step]; //Console.WriteLine("NR8ByteIO:" + NR8ByteIO); step++; for (int k = 0; k < NR8ByteIO; k++) { Telemetry tel = new Telemetry(); tel.ID = (Int32)data[step]; step++; for (int j = 0; j < 7; j++) { tel.TelemetryValue <<= 8; tel.TelemetryValue |= (Int32)data[step]; // Console.Write(" 0x" + data[step].ToString("X2")); step++; } sourceTelemetry.Add(tel); //Console.WriteLine("DI 8 byte: "); //step += 9; //1 byte ID 8 byte value step++; } #endregion //Console.WriteLine(); //Console.WriteLine("Time:"+itime70+" LONG:"+ilong+" LAT:"+ilat+" Speed:"+iSpeed); //add data to location QUEUE try { //locManager.SendLoc2messagebus(imei, (Int32)(itime70/1000), iSpeed.ToString(), pLat, pLong); htCell_t cell = new htCell_t(); cell.suid = imei; cell.spd = iSpeed.ToString(); cell.lat = pLat; cell.lng = pLong; cell.d_lat = dlat; cell.d_lng = dlong; cell.location_time = Utils.UnixTimeStampToDateTime(itime70 / 1000); cell.telemetryList = new List(sourceTelemetry); Utils.ConsWrite(DebugMSG_Type.GPS, "GPS for SUID: " + imei + " with time: " + cell.location_time + " lat: " + pLat + " lng: " + pLong); LOGS.LOG(DateTime.Now.ToString() + " : GPS for SUID: " + imei + " with time: " + cell.location_time + " lat: " + pLat + " lng: " + pLong); SN_Queues.DBQueueLocation.PostItem(cell); Utils.ConsWrite(DebugMSG_Type.GPS, "Message added to queue!!!"); LOGS.LOG(DateTime.Now.ToString() + " : Message added to queue!!!"); //process ALERTS if (SN_Queues.ht_SUInfo.ContainsKey(imei)) { Utils.ConsWrite(DebugMSG_Type.ALERTS, "Processing alerts for: " + imei); Utils.ConsWrite(DebugMSG_Type.ALERTS, "Message mask: " + DImask); SUinfo sui = (SUinfo)SN_Queues.ht_SUInfo[imei]; foreach (Alert alert in sui.alertList) { if (alert.Type == Alert_TYPE.DI) if (alert.DImask1 == DImask) { alert.Alert_latitude = pLat; alert.Alert_longitude = pLong; alert.Speed = iSpeed; alert.Digital_in = DImask; alert.Position_time = Utils.UnixTimeStampToDateTime(itime70 / 1000).ToString("yyyy:MM:dd HH:mm:ss"); SN_Queues.alertQueue.PostItem(alert); Utils.ConsWrite(DebugMSG_Type.ALERTS, "Alert: " + alert.Alert_name + " added to queue"); LOGS.LOG(DateTime.Now.ToString() + " : Alert: " + alert.Alert_name + " added to queue"); } } } } catch (Exception ex) { Utils.ConsWrite(DebugMSG_Type.always, "ConnectionThread.cs =>> Error adding location to queue!!!"); Utils.ConsWrite(DebugMSG_Type.always, ex.ToString()); } } return numberofMessByte; } public static Byte[] getByteCommand(string str) { #region Command string hex = ""; foreach (char letter in str) { hex += String.Format("{0:X}", Convert.ToInt32(letter)); } string chunk = ""; List sendByteArray = new List(); foreach (char c in hex) { chunk += c; if (chunk.Length == 2) { sendByteArray.Add((byte)(Int32.Parse(chunk, System.Globalization.NumberStyles.HexNumber))); chunk = ""; } } #endregion Command #region Carriage Return + Line feed sendByteArray.Add(0x0D); sendByteArray.Add(0x0A); #endregion Carriage Return + Line feed #region Quantity of commands sendByteArray.Add(0x01); #endregion Quantity of commands #region Command size int command_Size = sendByteArray.ToArray().Length; sendByteArray.Insert(0, (byte)command_Size); sendByteArray.Insert(0, 0x00); sendByteArray.Insert(0, 0x00); sendByteArray.Insert(0, 0x00); #endregion Command size #region Command type sendByteArray.Insert(0, 0x05); #endregion Command type #region Quantity of commands sendByteArray.Insert(0, 0x01); #endregion Quantity of commands #region Codec sendByteArray.Insert(0, 0x0C); #endregion Codec #region packet length int packet_length = sendByteArray.ToArray().Length; byte[] bytes = new byte[4]; bytes[0] = (byte)(packet_length >> 24); bytes[1] = (byte)(packet_length >> 16); bytes[2] = (byte)(packet_length >> 8); bytes[3] = (byte)packet_length; sendByteArray.Insert(0, bytes[3]); sendByteArray.Insert(0, bytes[2]); sendByteArray.Insert(0, bytes[1]); sendByteArray.Insert(0, bytes[0]); #endregion packet length #region Preamble sendByteArray.Insert(0, 0x00); sendByteArray.Insert(0, 0x00); sendByteArray.Insert(0, 0x00); sendByteArray.Insert(0, 0x00); List newList = sendByteArray.GetRange(8, sendByteArray.ToArray().Length - 8); byte[] byteArray = newList.ToArray(); #endregion Preamble #region crc UInt16 crc = Crc16.ComputeChecksum(byteArray); bytes = new byte[4]; bytes[0] = (byte)(crc >> 24); bytes[1] = (byte)(crc >> 16); bytes[2] = (byte)(crc >> 8); bytes[3] = (byte)crc; sendByteArray.Add(bytes[0]); sendByteArray.Add(bytes[1]); sendByteArray.Add(bytes[2]); sendByteArray.Add(bytes[3]); #endregion crc return sendByteArray.ToArray(); } public static string getDeviceResponse(byte[] data) { string response = string.Empty; //ignoring preamble & packet length Int32 step = 8; int codec = data[step++]; if (codec != 12) return response; int qtyOfCommands = data[step++]; if (qtyOfCommands != 1) return response; int commandType = data[step++]; if (commandType != 6) return response; int messageLength = 0; for (int j = 0; j < 4; j++) { messageLength <<= 8; messageLength |= data[step]; step++; } response = System.Text.Encoding.ASCII.GetString(data, step, messageLength).Trim(); return response; } //CRC-16-ANSI LSB-reverse (with 0xA001 coefficient) //CRC-16-ANSI MSB-normal (with 0x8005 coefficient) //CRC-XMODEM LSB-reverse (with 0x8408 coefficient) //CRC-CCITT MSB-normal (with 0x1021 coefficient) } #region CRC 16 0xA001 //http://www.goobbe.biz/questions/5764811/how-to-generate-a-crc-16-from-c-sharp public static class Crc16 { const ushort polynomial = 0xA001; static readonly ushort[] table = new ushort[256]; public static ushort ComputeChecksum(byte[] bytes) { ushort crc = 0; for (int i = 0; i < bytes.Length; ++i) { byte index = (byte)(crc ^ bytes[i]); crc = (ushort)((crc >> 8) ^ table[index]); } return crc; } static Crc16() { ushort value; ushort temp; for (ushort i = 0; i < table.Length; ++i) { value = 0; temp = i; for (byte j = 0; j < 8; ++j) { if (((value ^ temp) & 0x0001) != 0) { value = (ushort)((value >> 1) ^ polynomial); } else { value >>= 1; } temp >>= 1; } table[i] = value; } } } #endregion }