Skip to content

Commit

Permalink
Bugfix/mstp extended frames client (bacnet-stack#592)
Browse files Browse the repository at this point in the history 
* Fix routing to a remote network in the router-mstp example.

The net, len and adr fields of remote_dest are not initialized
although they are used by the npdu_encode_pdu function in
case of "Routing to another Router". Thank you, Eugene!

* Fixed handling of received MS/TP extended frames.

mstp_port->DataLength should be set to the actual length of decoded data.

* Fix MSTP_Master_Node_FSM and MSTP_Slave_Node_FSM for extended frames.

BACNET_EXTENDED_DATA frames should be treated the same as
their corresponding BACNET_DATA frames.

* Fixed MSTP COBS frame encoding

cobs_frame_encode writes the encoded data to the
beginning of the buffer, overwriting the frame header.

The frame header is constructed before COBS frame encoding,
so it contains the wrong Frame Type and Data Length.

* Added extended frame client unit test

* Fix router-ipv6 application for remote networks
  • Loading branch information
@skarg
skarg authored Mar 7, 2024
1 parent 4e94323 commit 2ecb1a2
Show file tree
Hide file tree
Showing 4 changed files with 91 additions and 45 deletions.
3 changes: 2 additions & 1 deletion apps/router-ipv6/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -865,6 +865,7 @@ static void routed_apdu_handler(uint16_t snet,
}
return;
}
remote_dest = *dest;
port = dnet_find(dest->net, &remote_dest);
if (port) {
if (port->net == dest->net) {
Expand Down Expand Up @@ -917,7 +918,7 @@ static void routed_apdu_handler(uint16_t snet,
port = Router_Table_Head;
while (port != NULL) {
if (port->net != snet) {
datalink_send_pdu(port->net, dest, npdu, &Tx_Buffer[0],
datalink_send_pdu(port->net, dest, npdu, &Tx_Buffer[0],
npdu_len + apdu_len);
}
port = port->next;
Expand Down
1 change: 1 addition & 0 deletions apps/router-mstp/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -891,6 +891,7 @@ static void routed_apdu_handler(uint16_t snet,
}
return;
}
remote_dest = *dest;
port = dnet_find(dest->net, &remote_dest);
if (port) {
if (port->net == dest->net) {
Expand Down
83 changes: 42 additions & 41 deletions src/bacnet/datalink/mstp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,24 +202,10 @@ uint16_t MSTP_Create_Frame(uint8_t *buffer,
uint16_t cobs_len; /* length of the COBS encoded frame */
bool cobs_bacnet_frame = false; /* true for COBS BACnet frames */

/* not enough to do a header */
if (buffer && (buffer_size >= 8)) {
buffer[0] = 0x55;
buffer[1] = 0xFF;
buffer[2] = frame_type;
crc8 = CRC_Calc_Header(buffer[2], crc8);
buffer[3] = destination;
crc8 = CRC_Calc_Header(buffer[3], crc8);
buffer[4] = source;
crc8 = CRC_Calc_Header(buffer[4], crc8);
buffer[5] = data_len >> 8; /* MSB first */
crc8 = CRC_Calc_Header(buffer[5], crc8);
buffer[6] = data_len & 0xFF;
crc8 = CRC_Calc_Header(buffer[6], crc8);
buffer[7] = ~crc8;
if (!buffer) {
return 0;
}
index = 8;

/* encode the data portion of the packet */
if ((data_len > MSTP_FRAME_NPDU_MAX) ||
((frame_type >= Nmin_COBS_type) && (frame_type <= Nmax_COBS_type))) {
/* COBS encoded frame */
Expand All @@ -234,7 +220,8 @@ uint16_t MSTP_Create_Frame(uint8_t *buffer,
/* I'm sorry, Dave, I'm afraid I can't do that. */
return 0;
}
cobs_len = cobs_frame_encode(buffer, buffer_size, data, data_len);
cobs_len = cobs_frame_encode(&buffer[8], buffer_size-8, data,
data_len);
/* check the results of COBs encoding for validity */
if (cobs_bacnet_frame) {
if (cobs_len < Nmin_COBS_length_BACnet) {
Expand All @@ -253,29 +240,38 @@ uint16_t MSTP_Create_Frame(uint8_t *buffer,
MS/TP frame length field since CRC32 is 2 bytes longer
than CRC16 in original MSTP and non-COBS devices need
to be able to ingest the entire frame */
index = index + cobs_len - 2;
data_len = cobs_len - 2;
} else if (data_len > 0) {
while (data_len && data && (index < buffer_size)) {
if (buffer) {
buffer[index] = *data;
crc16 = CRC_Calc_Data(buffer[index], crc16);
}
data++;
index++;
data_len--;
}
if ((index + 2) > buffer_size) {
if (!data) {
return 0;
}
crc16 = ~crc16;
if (buffer) {
buffer[index] = crc16 & 0xFF; /* LSB first */
if ((8 + data_len + 2) > buffer_size) {
return 0;
}
index++;
if (buffer) {
buffer[index] = crc16 >> 8;
for (index = 8; index < (data_len + 8); index++, data++) {
buffer[index] = *data;
crc16 = CRC_Calc_Data(buffer[index], crc16);
}
index++;
crc16 = ~crc16;
buffer[index] = crc16 & 0xFF; /* LSB first */
buffer[index+1] = crc16 >> 8;
}
buffer[0] = 0x55;
buffer[1] = 0xFF;
buffer[2] = frame_type;
crc8 = CRC_Calc_Header(buffer[2], crc8);
buffer[3] = destination;
crc8 = CRC_Calc_Header(buffer[3], crc8);
buffer[4] = source;
crc8 = CRC_Calc_Header(buffer[4], crc8);
buffer[5] = data_len >> 8; /* MSB first */
crc8 = CRC_Calc_Header(buffer[5], crc8);
buffer[6] = data_len & 0xFF;
crc8 = CRC_Calc_Header(buffer[6], crc8);
buffer[7] = ~crc8;
index = 8;
if (data_len > 0) {
index += data_len + 2;
}

return index; /* returns the frame length */
Expand Down Expand Up @@ -575,18 +571,19 @@ void MSTP_Receive_Frame_FSM(struct mstp_port_struct_t *mstp_port)
if (((mstp_port->Index + 1) < mstp_port->InputBufferSize) &&
(mstp_port->FrameType >= Nmin_COBS_type) &&
(mstp_port->FrameType <= Nmax_COBS_type)) {
if (cobs_frame_decode(
&mstp_port->InputBuffer[mstp_port->Index + 1],
mstp_port->InputBufferSize,
mstp_port->InputBuffer, mstp_port->Index + 1)) {
mstp_port->DataLength = cobs_frame_decode(
&mstp_port->InputBuffer[mstp_port->Index + 1],
mstp_port->InputBufferSize,
mstp_port->InputBuffer, mstp_port->Index + 1);
if (mstp_port->DataLength > 0) {
mstp_port->ReceivedValidFrame = true;
} else {
mstp_port->ReceivedInvalidFrame = true;
}
} else {
/* STATE DATA CRC - no need for new state */
if (mstp_port->DataCRC == 0xF0B8) {
/* indicate the complete reception of a
/* indicate the complete reception of a
valid frame */
mstp_port->ReceivedValidFrame = true;
} else {
Expand Down Expand Up @@ -727,6 +724,7 @@ bool MSTP_Master_Node_FSM(struct mstp_port_struct_t *mstp_port)
}
break;
case FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY:
case FRAME_TYPE_BACNET_EXTENDED_DATA_NOT_EXPECTING_REPLY:
if ((mstp_port->DestinationAddress ==
MSTP_BROADCAST_ADDRESS) &&
(npdu_confirmed_service(mstp_port->InputBuffer,
Expand All @@ -743,6 +741,7 @@ bool MSTP_Master_Node_FSM(struct mstp_port_struct_t *mstp_port)
}
break;
case FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY:
case FRAME_TYPE_BACNET_EXTENDED_DATA_EXPECTING_REPLY:
if (mstp_port->DestinationAddress ==
MSTP_BROADCAST_ADDRESS) {
/* broadcast DER just remains IDLE */
Expand Down Expand Up @@ -1241,6 +1240,7 @@ void MSTP_Slave_Node_FSM(struct mstp_port_struct_t *mstp_port)
mstp_port->ReceivedValidFrame = false;
switch (mstp_port->FrameType) {
case FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY:
case FRAME_TYPE_BACNET_EXTENDED_DATA_EXPECTING_REPLY:
if (mstp_port->DestinationAddress != MSTP_BROADCAST_ADDRESS) {
/* indicate successful reception to the higher layers */
(void)MSTP_Put_Receive(mstp_port);
Expand All @@ -1255,6 +1255,7 @@ void MSTP_Slave_Node_FSM(struct mstp_port_struct_t *mstp_port)
case FRAME_TYPE_POLL_FOR_MASTER:
case FRAME_TYPE_TEST_RESPONSE:
case FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY:
case FRAME_TYPE_BACNET_EXTENDED_DATA_NOT_EXPECTING_REPLY:
default:
break;
}
Expand Down
49 changes: 46 additions & 3 deletions test/bacnet/datalink/mstp/src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include <zephyr/ztest.h>
#include <bacnet/bytes.h>
#include <bacnet/datalink/crc.h>
#include <bacnet/datalink/cobs.h>
#include <bacnet/datalink/datalink.h>
#include <bacnet/datalink/mstp.h>
#include <bacnet/datalink/mstpdef.h>
Expand Down Expand Up @@ -507,7 +508,49 @@ static void testReceiveNodeFSM(void)
zassert_true(mstp_port.ReceivedInvalidFrame == false, NULL);
zassert_true(mstp_port.ReceivedValidFrame == true, NULL);
zassert_true(mstp_port.receive_state == MSTP_RECEIVE_STATE_IDLE, NULL);
return;
/* Extended-Data-Expecting-Reply */
zassert_true(Nmin_COBS_length_BACnet <= sizeof(data), NULL);
mstp_port.ReceivedInvalidFrame = false;
mstp_port.ReceivedValidFrame = false;
memset(data, 0, sizeof(data));
len = MSTP_Create_Frame(buffer, sizeof(buffer),
FRAME_TYPE_BACNET_EXTENDED_DATA_EXPECTING_REPLY,
my_mac, my_mac, data, Nmin_COBS_length_BACnet);
zassert_true(len > 0, NULL);
Load_Input_Buffer(buffer, len);
RS485_Check_UART_Data(&mstp_port);
MSTP_Receive_Frame_FSM(&mstp_port);
while (mstp_port.receive_state != MSTP_RECEIVE_STATE_IDLE) {
RS485_Check_UART_Data(&mstp_port);
MSTP_Receive_Frame_FSM(&mstp_port);
}
zassert_true(mstp_port.DataLength == Nmin_COBS_length_BACnet, NULL);
zassert_true(mstp_port.ReceivedInvalidFrame == false, NULL);
zassert_true(mstp_port.ReceivedValidFrame == true, NULL);
zassert_true(mstp_port.receive_state == MSTP_RECEIVE_STATE_IDLE, NULL);
zassert_true(mstp_port.FrameType ==
FRAME_TYPE_BACNET_EXTENDED_DATA_EXPECTING_REPLY, NULL);
/* Extended-Data-Not-Expecting-Reply */
mstp_port.ReceivedInvalidFrame = false;
mstp_port.ReceivedValidFrame = false;
memset(data, 0, sizeof(data));
len = MSTP_Create_Frame(buffer, sizeof(buffer),
FRAME_TYPE_BACNET_EXTENDED_DATA_NOT_EXPECTING_REPLY,
my_mac, my_mac, data, Nmin_COBS_length_BACnet);
zassert_true(len > 0, NULL);
Load_Input_Buffer(buffer, len);
RS485_Check_UART_Data(&mstp_port);
MSTP_Receive_Frame_FSM(&mstp_port);
while (mstp_port.receive_state != MSTP_RECEIVE_STATE_IDLE) {
RS485_Check_UART_Data(&mstp_port);
MSTP_Receive_Frame_FSM(&mstp_port);
}
zassert_true(mstp_port.DataLength == Nmin_COBS_length_BACnet, NULL);
zassert_true(mstp_port.ReceivedInvalidFrame == false, NULL);
zassert_true(mstp_port.ReceivedValidFrame == true, NULL);
zassert_true(mstp_port.receive_state == MSTP_RECEIVE_STATE_IDLE, NULL);
zassert_true(mstp_port.FrameType ==
FRAME_TYPE_BACNET_EXTENDED_DATA_NOT_EXPECTING_REPLY, NULL);
}

static void testMasterNodeFSM(void)
Expand Down Expand Up @@ -1009,14 +1052,14 @@ static void testZeroConfigNodeFSM(void)
testZeroConfigNode_Init(&MSTP_Port);
testZeroConfigNode_Test_IDLE_ValidFrame(&MSTP_Port);
testZeroConfigNode_Test_LURK_LearnMaxMaster(&MSTP_Port);
/* test case: valid frame event LURK PFMs: ClaimAddress
/* test case: valid frame event LURK PFMs: ClaimAddress
ConfirmationSuccessful */
testZeroConfigNode_Init(&MSTP_Port);
testZeroConfigNode_Test_IDLE_ValidFrame(&MSTP_Port);
testZeroConfigNode_Test_LURK_Claim(&MSTP_Port);
testZeroConfigNode_Test_LURK_ClaimTokenForUs(&MSTP_Port);
testZeroConfigNode_Test_LURK_ConfirmationSuccessful(&MSTP_Port);
/* test case: valid frame event LURK PFMs: ClaimAddress
/* test case: valid frame event LURK PFMs: ClaimAddress
ConfirmationAddressInUse */
testZeroConfigNode_Init(&MSTP_Port);
testZeroConfigNode_Test_IDLE_ValidFrame(&MSTP_Port);
Expand Down

0 comments on commit 2ecb1a2

Please sign in to comment.