heat_control/esp_server/ModbusServer.cs

using NLog;
using System;
using System.Collections.Generic;
using System.IO;
using System.Net;
using System.Net.Sockets;
using System.Threading;

namespace ESPServer
{
	class ModbusServer
	{
		private RegistersContainer registers;

		public int Port { get; set; }

		public ModbusServer(RegistersContainer registers)
		{
			this.registers = registers;
		}

		internal void Run()
		{
			var logger = LogManager.GetCurrentClassLogger();
			logger.Info("Accepting connections on port " + Port);
			var listener = new TcpListener(IPAddress.Any, Port);
			listener.Start();
			while (true)
			{
				var client = listener.AcceptTcpClient();
				logger.Info("Accepted connection from " + client.Client.RemoteEndPoint);
				var worker = new ModbusServerWorker(registers, client);
				Thread clientThread = new Thread(new ThreadStart(worker.Run));
				clientThread.Start();
			}
		}
	}

	class ModbusServerWorker
	{
		const int RECEIVE_TIMEOUT = 10000; // 10 seconds

		private RegistersContainer registers;
		private TcpClient client;
		private static Logger logger = LogManager.GetCurrentClassLogger();
		public ModbusServerWorker(RegistersContainer registers, TcpClient client)
		{
			this.registers = registers;
			this.client = client;
		}

		List<byte> received;
		ManualResetEvent readCompleted;
		int currentReceived;
		NetworkStream stream;
		byte[] buffer = new byte[256];

		public void Run()
		{
			logger.Debug("Working with client");
			received = new List<byte>();
			try
			{
				client.ReceiveTimeout = RECEIVE_TIMEOUT;
				stream = client.GetStream();
				readCompleted = new ManualResetEvent(false);
				while (true)
				{
					readCompleted.Reset();
					stream.BeginRead(buffer, 0, buffer.Length, new AsyncCallback(this.OnReadCompleted), this);
					readCompleted.WaitOne();
					if (currentReceived <= 0)
					{
						client.Close();
						return;
					}
					ProcessReceivedData();
				}
			}
			catch (IOException)
			{
				client.Close();
			}
			finally
			{
				if (stream != null) stream.Dispose();
				client.Dispose();
			}
		}

		private void ProcessReceivedData()
		{
			lock (received)
			{
				if (received.Count < 6) return;
				int len = (((int)received[4] << 8) | (int)received[5]);
				if (received.Count < 6 + len) return;
				if (len < 2) throw new Exception("Wrong length");
				int transactionId = (((int)received[0] << 8) | (int)received[1]);
				byte unitId = received[6];
				byte function = received[7];
				List<byte> response = new List<byte>();
				if (function == 0x03) // read holding registers
				{
					int addr = (((int)received[8] << 8) | (int)received[9]);
					int amount = (((int)received[10] << 8) | (int)received[11]);
					for (int a = addr; a < addr + amount; a++)
					{
						int value = registers[a];
						response.Add((byte)((value >> 8) & 0xFF));
						response.Add((byte)(value & 0xFF));
					}
				}
				else
				{
					throw new Exception("Unsupported function: " + function);
				}
				List<byte> fullResponse = new List<byte>();
				fullResponse.Add(received[0]);
				fullResponse.Add(received[1]); // transaction id
				fullResponse.Add(received[2]);
				fullResponse.Add(received[3]); // protocol id
				int responseLength = 3 + response.Count;
				fullResponse.Add((byte)((responseLength >> 8) & 0xFF));
				fullResponse.Add((byte)((responseLength) & 0xFF));
				fullResponse.Add(unitId);
				fullResponse.Add(function);
				fullResponse.Add((byte)response.Count);
				fullResponse.AddRange(response);
				received.RemoveRange(0, 6 + len);
				stream.Write(fullResponse.ToArray(), 0, fullResponse.Count);
			}
		}

		private void OnReadCompleted(IAsyncResult ar)
		{
			try
			{
				currentReceived = stream.EndRead(ar);
				lock (received)
				{
					for (int i = 0; i < currentReceived; i++) received.Add(buffer[i]);
				}
				readCompleted.Set();
			}
			catch (IOException e)
			{
				logger.Info("Error receiving " + e);
				currentReceived = -1;
			}
		}
	}
}