nc_program_manager/Cnc/Heidenhain/Lsv2Client.cs

using System;
using System.Net.Sockets;
using System.Text;

namespace FanucProgramManager.Cnc.Heidenhain
{
    /// <summary>LSV2 protocol client for Heidenhain TNC controls.</summary>
    internal sealed class Lsv2Client : ILsv2Client, IDisposable
    {
        private TcpClient _tcp;
        private NetworkStream _stream;
        private readonly HeidenhainConnectionConfig _config;

        private const ushort T_OK         = 0x0000;
        private const ushort T_SE         = 0x0001;
        private const ushort T_LS_SELECT  = 0x00A0;
        private const ushort T_LS_LOG_IN  = 0x00A1;
        private const ushort T_LS_LOG_OUT = 0x00A2;
        private const ushort T_R_FL       = 0x0023;
        private const ushort T_S_FL       = 0x0024;
        private const ushort T_FD         = 0x0025;

        public Lsv2Client(HeidenhainConnectionConfig config)
        {
            _config = config;
        }

        public bool Connect()
        {
            _tcp = new TcpClient();
            _tcp.SendTimeout    = (int)_config.ConnectTimeout.TotalMilliseconds;
            _tcp.ReceiveTimeout = (int)_config.ConnectTimeout.TotalMilliseconds;
            if (!_tcp.ConnectAsync(_config.IpAddress, _config.Port).Wait(_config.ConnectTimeout))
                return false;
            _stream = _tcp.GetStream();
            return Login();
        }

        public void Disconnect()
        {
            try { SendCommand(T_LS_LOG_OUT, new byte[0]); } catch { }
            try { if (_stream != null) _stream.Close(); } catch { }
            try { if (_tcp   != null) _tcp.Close();    } catch { }
        }

        /// <summary>Upload file from NC to PC. Returns file content or null on failure.</summary>
        public string ReceiveFile(string ncPath)
        {
            byte[] pathBytes = Encoding.ASCII.GetBytes(ncPath + "\0");
            SendCommand(T_R_FL, pathBytes);

            var sb = new StringBuilder();
            while (true)
            {
                ushort cmd;
                byte[] data = ReadFrame(out cmd);
                if (cmd == T_SE || data == null) return null;
                if (cmd == T_OK) break;
                if (cmd == T_FD) sb.Append(Encoding.ASCII.GetString(data));
            }
            return sb.ToString();
        }

        /// <summary>Download file from PC to NC.</summary>
        public bool SendFile(string ncPath, string content)
        {
            byte[] pathBytes    = Encoding.ASCII.GetBytes(ncPath + "\0");
            byte[] contentBytes = Encoding.ASCII.GetBytes(content);
            byte[] payload      = new byte[pathBytes.Length + contentBytes.Length];
            Buffer.BlockCopy(pathBytes,    0, payload, 0,               pathBytes.Length);
            Buffer.BlockCopy(contentBytes, 0, payload, pathBytes.Length, contentBytes.Length);
            SendCommand(T_S_FL, payload);

            ushort cmd;
            ReadFrame(out cmd);
            return cmd == T_OK;
        }

        public void Dispose() => Disconnect();

        // ── private ──────────────────────────────────────────────────────────

        private bool Login()
        {
            // Select programming mode (3)
            byte[] mode = { 0x03 };
            SendCommand(T_LS_SELECT, mode);
            ushort cmd;
            ReadFrame(out cmd);
            if (cmd != T_OK) return false;

            // Authenticate
            byte[] user = Encoding.ASCII.GetBytes(_config.Username + "\0");
            byte[] pass = Encoding.ASCII.GetBytes(_config.Password + "\0");
            byte[] cred = new byte[user.Length + pass.Length];
            Buffer.BlockCopy(user, 0, cred, 0,           user.Length);
            Buffer.BlockCopy(pass, 0, cred, user.Length, pass.Length);
            SendCommand(T_LS_LOG_IN, cred);
            ReadFrame(out cmd);
            return cmd == T_OK;
        }

        private void SendCommand(ushort cmdCode, byte[] data)
        {
            int    dataLen = data.Length;
            ushort size    = (ushort)(2 + dataLen);

            byte sizeHi = (byte)(size   >> 8);
            byte sizeLo = (byte)(size   & 0xFF);
            byte cmdHi  = (byte)(cmdCode >> 8);
            byte cmdLo  = (byte)(cmdCode & 0xFF);

            byte[] crcInput = new byte[4 + dataLen];
            crcInput[0] = sizeHi; crcInput[1] = sizeLo;
            crcInput[2] = cmdHi;  crcInput[3] = cmdLo;
            Buffer.BlockCopy(data, 0, crcInput, 4, dataLen);
            ushort crc = Crc16(crcInput);

            byte[] frame = new byte[1 + 4 + dataLen + 2];
            int i = 0;
            frame[i++] = 0x02;
            frame[i++] = sizeHi;
            frame[i++] = sizeLo;
            frame[i++] = cmdHi;
            frame[i++] = cmdLo;
            Buffer.BlockCopy(data, 0, frame, i, dataLen);
            i += dataLen;
            frame[i++] = (byte)(crc >> 8);
            frame[i]   = (byte)(crc & 0xFF);

            _stream.Write(frame, 0, frame.Length);
        }

        private byte[] ReadFrame(out ushort cmd)
        {
            cmd = 0xFFFF;
            int stx = _stream.ReadByte();
            if (stx != 0x02) return null;

            byte sizeHi = (byte)_stream.ReadByte();
            byte sizeLo = (byte)_stream.ReadByte();
            int  size   = (sizeHi << 8) | sizeLo;
            if (size < 2) return null;

            byte cmdHi = (byte)_stream.ReadByte();
            byte cmdLo = (byte)_stream.ReadByte();
            cmd = (ushort)((cmdHi << 8) | cmdLo);

            int    dataLen = size - 2;
            byte[] data    = new byte[dataLen];
            int    read    = 0;
            while (read < dataLen)
            {
                int n = _stream.Read(data, read, dataLen - read);
                if (n == 0) break;
                read += n;
            }

            // consume CRC bytes
            _stream.ReadByte();
            _stream.ReadByte();

            return data;
        }

        private static ushort Crc16(byte[] data)
        {
            ushort crc = 0xFFFF;
            foreach (byte b in data)
            {
                crc ^= (ushort)(b << 8);
                for (int i = 0; i < 8; i++)
                    crc = (crc & 0x8000) != 0
                        ? (ushort)((crc << 1) ^ 0x1021)
                        : (ushort)(crc << 1);
            }
            return crc;
        }
    }
}