public static int Decode(byte[] buffer, int offset, int max_length, out BacnetMstpFrameTypes frame_type, out byte destination_address, out byte source_address, out int msg_length)
{
frame_type = (BacnetMstpFrameTypes)buffer[offset + 2];
destination_address = buffer[offset + 3];
source_address = buffer[offset + 4];
msg_length = (buffer[offset + 5] << 8) | (buffer[offset + 6] << 0);
byte crc_header = buffer[offset + 7];
ushort crc_data = 0;
if (max_length < MSTP_HEADER_LENGTH)
{
return(-1); //not enough data
}
if (msg_length > 0)
{
crc_data = (ushort)((buffer[offset + 8 + msg_length + 1] << 8) | (buffer[offset + 8 + msg_length + 0] << 0));
}
if (buffer[offset + 0] != MSTP_PREAMBLE1)
{
return(-1);
}
if (buffer[offset + 1] != MSTP_PREAMBLE2)
{
return(-1);
}
if (CRC_Calc_Header(buffer, offset + 2, 5) != crc_header)
{
return(-1);
}
if (msg_length > 0 && max_length >= (MSTP_HEADER_LENGTH + msg_length + 2) && CRC_Calc_Data(buffer, offset + 8, msg_length) != crc_data)
{
return(-1);
}
return(8 + (msg_length) + (msg_length > 0 ? 2 : 0));
}
public int Send(byte[] buffer, int offset, int data_length, BacnetAddress address, bool wait_for_transmission, int timeout)
{
if (m_TS == -1)
{
throw new Exception("Source address must be set up before sending messages");
}
//add to queue
BacnetNpduControls function = NPDU.DecodeFunction(buffer, offset);
BacnetMstpFrameTypes frame_type = (function & BacnetNpduControls.ExpectingReply) == BacnetNpduControls.ExpectingReply ? BacnetMstpFrameTypes.FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY : BacnetMstpFrameTypes.FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
byte[] copy = new byte[data_length + MSTP.MSTP_HEADER_LENGTH + 2];
Array.Copy(buffer, offset, copy, MSTP.MSTP_HEADER_LENGTH, data_length);
MessageFrame f = new MessageFrame(frame_type, address.adr[0], copy, data_length);
lock (m_send_queue)
m_send_queue.AddLast(f);
if (m_reply == null)
{
m_reply = f;
m_reply_mutex.Set();
}
//wait for message to be sent
if (wait_for_transmission)
{
if (!f.send_mutex.WaitOne(timeout))
{
return(-ETIMEDOUT);
}
}
return(data_length);
}
public MessageFrame(BacnetMstpFrameTypes frameType, byte destinationAddress, byte[] data, int dataLength)
{
FrameType = frameType;
DestinationAddress = destinationAddress;
Data = data;
DataLength = dataLength;
SendMutex = new ManualResetEvent(false);
}
public MessageFrame(BacnetMstpFrameTypes frame_type, byte destination_address, byte[] data, int data_length)
{
this.frame_type = frame_type;
this.destination_address = destination_address;
this.data = data;
this.data_length = data_length;
send_mutex = new ManualResetEvent(false);
}
public static int Decode(byte[] buffer, int offset, int maxLength, out BacnetMstpFrameTypes frameType, out byte destinationAddress, out byte sourceAddress, out int msgLength)
{
if (maxLength < MSTP_HEADER_LENGTH) //not enough data
{
frameType = BacnetMstpFrameTypes.FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY; // don't care
destinationAddress = sourceAddress = 0; // don't care
msgLength = 0;
return(-1);
}
frameType = (BacnetMstpFrameTypes)buffer[offset + 2];
destinationAddress = buffer[offset + 3];
sourceAddress = buffer[offset + 4];
msgLength = (buffer[offset + 5] << 8) | (buffer[offset + 6] << 0);
var crcHeader = buffer[offset + 7];
ushort crcData = 0;
if (msgLength > 0)
{
if (offset + 8 + msgLength + 1 >= buffer.Length)
{
return(-1);
}
crcData = (ushort)((buffer[offset + 8 + msgLength + 1] << 8) | (buffer[offset + 8 + msgLength + 0] << 0));
}
if (buffer[offset + 0] != MSTP_PREAMBLE1)
{
return(-1);
}
if (buffer[offset + 1] != MSTP_PREAMBLE2)
{
return(-1);
}
if (CRC_Calc_Header(buffer, offset + 2, 5) != crcHeader)
{
return(-1);
}
if (msgLength > 0 && maxLength >= MSTP_HEADER_LENGTH + msgLength + 2 && CRC_Calc_Data(buffer, offset + 8, msgLength) != crcData)
{
return(-1);
}
return(8 + msgLength + (msgLength > 0 ? 2 : 0));
}
public static int Encode(byte[] buffer, int offset, BacnetMstpFrameTypes frameType, byte destinationAddress, byte sourceAddress, int msgLength)
{
buffer[offset + 0] = MSTP_PREAMBLE1;
buffer[offset + 1] = MSTP_PREAMBLE2;
buffer[offset + 2] = (byte)frameType;
buffer[offset + 3] = destinationAddress;
buffer[offset + 4] = sourceAddress;
buffer[offset + 5] = (byte)((msgLength & 0xFF00) >> 8);
buffer[offset + 6] = (byte)((msgLength & 0x00FF) >> 0);
buffer[offset + 7] = CRC_Calc_Header(buffer, offset + 2, 5);
if (msgLength > 0)
{
//calculate data crc
var dataCrc = CRC_Calc_Data(buffer, offset + 8, msgLength);
buffer[offset + 8 + msgLength + 0] = (byte)(dataCrc & 0xFF); //LSB first!
buffer[offset + 8 + msgLength + 1] = (byte)(dataCrc >> 8);
}
//optional pad (0xFF)
return(MSTP_HEADER_LENGTH + msgLength + (msgLength > 0 ? 2 : 0));
}
private GetMessageStatus GetNextMessage(int timeout_ms, out BacnetMstpFrameTypes frame_type, out byte destination_address, out byte source_address, out int msg_length)
{
int timeout;
frame_type = BacnetMstpFrameTypes.FRAME_TYPE_TOKEN;
destination_address = 0;
source_address = 0;
msg_length = 0;
//fetch header
while (m_local_offset < MSTP.MSTP_HEADER_LENGTH)
{
if (m_port == null) return GetMessageStatus.ConnectionClose;
if (m_local_offset > 0)
timeout = T_FRAME_ABORT; //set sub timeout
else
timeout = timeout_ms; //set big silence timeout
//read
int rx = m_port.Read(m_local_buffer, m_local_offset, MSTP.MSTP_HEADER_LENGTH - m_local_offset, timeout);
if (rx == -ETIMEDOUT)
{
//drop message
GetMessageStatus status = m_local_offset == 0 ? GetMessageStatus.Timeout : GetMessageStatus.SubTimeout;
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return status;
}
else if (rx < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.ConnectionError;
}
else if (rx == 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.ConnectionClose;
}
m_local_offset += rx;
//remove paddings & garbage
RemoveGarbage();
}
//decode
if (MSTP.Decode(m_local_buffer, 0, m_local_offset, out frame_type, out destination_address, out source_address, out msg_length) < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.DecodeError;
}
//valid length?
int full_msg_length = msg_length + MSTP.MSTP_HEADER_LENGTH + (msg_length > 0 ? 2 : 0);
if (msg_length > MaxBufferLength)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.DecodeError;
}
//fetch data
if (msg_length > 0)
{
timeout = T_FRAME_ABORT; //set sub timeout
while (m_local_offset < full_msg_length)
{
//read
int rx = m_port.Read(m_local_buffer, m_local_offset, full_msg_length - m_local_offset, timeout);
if (rx == -ETIMEDOUT)
{
//drop message
GetMessageStatus status = m_local_offset == 0 ? GetMessageStatus.Timeout : GetMessageStatus.SubTimeout;
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return status;
}
else if (rx < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.ConnectionError;
}
else if (rx == 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.ConnectionClose;
}
m_local_offset += rx;
}
//verify data crc
if (MSTP.Decode(m_local_buffer, 0, m_local_offset, out frame_type, out destination_address, out source_address, out msg_length) < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return GetMessageStatus.DecodeError;
}
}
//signal frame event
if (FrameRecieved != null)
{
BacnetMstpFrameTypes _frame_type = frame_type;
byte _destination_address = destination_address;
byte _source_address = source_address;
int _msg_length = msg_length;
System.Threading.ThreadPool.QueueUserWorkItem((o) => { FrameRecieved(this, _frame_type, _destination_address, _source_address, _msg_length); }, null);
}
if(StateLogging) Trace.WriteLine("" + frame_type + " " + destination_address.ToString("X2") + " ");
//done
return GetMessageStatus.Good;
}
public MessageFrame(BacnetMstpFrameTypes frame_type, byte destination_address, byte[] data, int data_length)
{
this.frame_type = frame_type;
this.destination_address = destination_address;
this.data = data;
this.data_length = data_length;
send_mutex = new Threading.ManualResetEvent(false);
}
private void SendFrame(BacnetMstpFrameTypes frame_type, byte destination_address)
{
SendFrame(new MessageFrame(frame_type, destination_address, null, 0));
}
private void QueueFrame(BacnetMstpFrameTypes frame_type, byte destination_address)
{
m_send_queue.AddLast(new MessageFrame(frame_type, destination_address, null, 0));
}
private GetMessageStatus GetNextMessage(int timeoutMs, out BacnetMstpFrameTypes frameType,
out byte destinationAddress, out byte sourceAddress, out int msgLength)
{
int timeout;
frameType = BacnetMstpFrameTypes.FRAME_TYPE_TOKEN;
destinationAddress = 0;
sourceAddress = 0;
msgLength = 0;
//fetch header
while (_localOffset < MSTP.MSTP_HEADER_LENGTH)
{
if (_port == null)
{
return(GetMessageStatus.ConnectionClose);
}
timeout = _localOffset > 0
? T_FRAME_ABORT // set sub timeout
: timeoutMs; // set big silence timeout
//read
var rx = _port.Read(_localBuffer, _localOffset, MSTP.MSTP_HEADER_LENGTH - _localOffset, timeout);
if (rx == -ETIMEDOUT)
{
//drop message
var status = _localOffset == 0 ? GetMessageStatus.Timeout : GetMessageStatus.SubTimeout;
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(status);
}
if (rx < 0)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionError);
}
if (rx == 0)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionClose);
}
_localOffset += rx;
//remove paddings & garbage
RemoveGarbage();
}
//decode
if (MSTP.Decode(_localBuffer, 0, _localOffset, out frameType, out destinationAddress,
out sourceAddress, out msgLength) < 0)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.DecodeError);
}
//valid length?
var fullMsgLength = msgLength + MSTP.MSTP_HEADER_LENGTH + (msgLength > 0 ? 2 : 0);
if (msgLength > MaxBufferLength)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.DecodeError);
}
//fetch data
if (msgLength > 0)
{
timeout = T_FRAME_ABORT; //set sub timeout
while (_localOffset < fullMsgLength)
{
//read
var rx = _port.Read(_localBuffer, _localOffset, fullMsgLength - _localOffset, timeout);
if (rx == -ETIMEDOUT)
{
//drop message
var status = _localOffset == 0 ? GetMessageStatus.Timeout : GetMessageStatus.SubTimeout;
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(status);
}
if (rx < 0)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionError);
}
if (rx == 0)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionClose);
}
_localOffset += rx;
}
//verify data crc
if (
MSTP.Decode(_localBuffer, 0, _localOffset, out frameType, out destinationAddress,
out sourceAddress, out msgLength) < 0)
{
//drop message
_localBuffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.DecodeError);
}
}
//signal frame event
if (FrameRecieved != null)
{
var frameTypeCopy = frameType;
var destinationAddressCopy = destinationAddress;
var sourceAddressCopy = sourceAddress;
var msgLengthCopy = msgLength;
ThreadPool.QueueUserWorkItem(
o => { FrameRecieved(this, frameTypeCopy, destinationAddressCopy, sourceAddressCopy, msgLengthCopy); }, null);
}
Log.Debug($"{frameType} {destinationAddress:X2}");
//done
return(GetMessageStatus.Good);
}
private void SendFrame(BacnetMstpFrameTypes frameType, byte destinationAddress)
{
SendFrame(new MessageFrame(frameType, destinationAddress, null, 0));
}
private void QueueFrame(BacnetMstpFrameTypes frameType, byte destinationAddress)
{
lock (_sendQueue)
_sendQueue.AddLast(new MessageFrame(frameType, destinationAddress, null, 0));
}
private GetMessageStatus GetNextMessage(int timeout_ms, out BacnetMstpFrameTypes frame_type, out byte destination_address, out byte source_address, out int msg_length)
{
int timeout;
frame_type = BacnetMstpFrameTypes.FRAME_TYPE_TOKEN;
destination_address = 0;
source_address = 0;
msg_length = 0;
//fetch header
while (m_local_offset < MSTP.MSTP_HEADER_LENGTH)
{
if (m_port == null)
{
return(GetMessageStatus.ConnectionClose);
}
if (m_local_offset > 0)
{
timeout = T_FRAME_ABORT; //set sub timeout
}
else
{
timeout = timeout_ms; //set big silence timeout
}
//read
int rx = m_port.Read(m_local_buffer, m_local_offset, MSTP.MSTP_HEADER_LENGTH - m_local_offset, timeout);
if (rx == -ETIMEDOUT)
{
//drop message
GetMessageStatus status = m_local_offset == 0 ? GetMessageStatus.Timeout : GetMessageStatus.SubTimeout;
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(status);
}
if (rx < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionError);
}
if (rx == 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionClose);
}
m_local_offset += rx;
//remove paddings & garbage
RemoveGarbage();
}
//decode
if (MSTP.Decode(m_local_buffer, 0, m_local_offset, out frame_type, out destination_address, out source_address, out msg_length) < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.DecodeError);
}
//valid length?
int full_msg_length = msg_length + MSTP.MSTP_HEADER_LENGTH + (msg_length > 0 ? 2 : 0);
if (msg_length > MaxBufferLength)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.DecodeError);
}
//fetch data
if (msg_length > 0)
{
timeout = T_FRAME_ABORT; //set sub timeout
while (m_local_offset < full_msg_length)
{
//read
int rx = m_port.Read(m_local_buffer, m_local_offset, full_msg_length - m_local_offset, timeout);
if (rx == -ETIMEDOUT)
{
//drop message
GetMessageStatus status = m_local_offset == 0 ? GetMessageStatus.Timeout : GetMessageStatus.SubTimeout;
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(status);
}
if (rx < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionError);
}
if (rx == 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.ConnectionClose);
}
m_local_offset += rx;
}
//verify data crc
if (MSTP.Decode(m_local_buffer, 0, m_local_offset, out frame_type, out destination_address, out source_address, out msg_length) < 0)
{
//drop message
m_local_buffer[0] = 0xFF;
RemoveGarbage();
return(GetMessageStatus.DecodeError);
}
}
//signal frame event
if (FrameRecieved != null)
{
BacnetMstpFrameTypes _frame_type = frame_type;
byte _destination_address = destination_address;
byte _source_address = source_address;
int _msg_length = msg_length;
ThreadPool.QueueUserWorkItem((o) => { FrameRecieved(this, _frame_type, _destination_address, _source_address, _msg_length); }, null);
}
if (StateLogging)
{
Trace.WriteLine("" + frame_type + " " + destination_address.ToString("X2") + " ");
}
//done
return(GetMessageStatus.Good);
}
