dali/bacnet_stack
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added postpone reply because transmission of the segmented ComplexACK cannot begin until the node holds the token. (#1116)

Browse Source
This commit is contained in:
Steve Karg
2025-11-15 13:08:27 -06:00
committed by GitHub
parent 51358e83f9
commit 9d1abbeba5
17 changed files with 539 additions and 1105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ports/at91sam7s/dlmstp.c
+4 -104
View File
@@ -195,106 +195,6 @@ void dlmstp_fill_bacnet_address(BACNET_ADDRESS *src, uint8_t mstp_address)
}
}
static bool dlmstp_compare_data_expecting_reply(const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = bacnet_npdu_decode(request_pdu, request_pdu_len, NULL,
&request.address, &request.npdu_data);
if (request.npdu_data.network_layer_message) {
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3))
request.service_choice = request_pdu[offset + 5];
else
request.service_choice = request_pdu[offset + 3];
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3))
reply.service_choice = reply_pdu[offset + 4];
else
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
if (request.invoke_id != reply.invoke_id) {
return false;
}
/* these services don't have service choice included */
if ((reply.pdu_type != PDU_TYPE_REJECT) &&
(reply.pdu_type != PDU_TYPE_ABORT) &&
(reply.pdu_type != PDU_TYPE_SEGMENT_ACK)) {
if (request.service_choice != reply.service_choice) {
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
return false;
}
return true;
}
/* MS/TP Frame Format */
/* All frames are of the following format: */
/* */
@@ -1063,9 +963,9 @@ static bool MSTP_Master_Node_FSM(void)
case MSTP_MASTER_STATE_ANSWER_DATA_REQUEST:
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
if (pkt != NULL) {
matched = dlmstp_compare_data_expecting_reply(&InputBuffer[0],
DataLength, SourceAddress, &pkt->buffer[0], pkt->length,
pkt->destination_mac);
matched = npdu_is_data_expecting_reply(
&InputBuffer[0], DataLength, SourceAddress,
&pkt->buffer[0], pkt->length, pkt->destination_mac);
} else {
matched = false;
}
@@ -1161,7 +1061,7 @@ static void MSTP_Slave_Node_FSM(void)
} else if (MSTP_Flag.ReceivePacketPending) {
if (!Ringbuf_Empty(&PDU_Queue)) {
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
matched = dlmstp_compare_data_expecting_reply(&InputBuffer[0],
matched = npdu_is_data_expecting_reply(&InputBuffer[0],
DataLength, SourceAddress, &pkt->buffer[0], pkt->length,
pkt->destination_mac);
if (matched) {
ports/bdk-atxx4-mstp/dlmstp.c
+2 -102
View File
@@ -196,106 +196,6 @@ void dlmstp_fill_bacnet_address(BACNET_ADDRESS *src, uint8_t mstp_address)
}
}
static bool dlmstp_compare_data_expecting_reply(const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = bacnet_npdu_decode(request_pdu, request_pdu_len, NULL,
&request.address, &request.npdu_data);
if (request.npdu_data.network_layer_message) {
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3))
request.service_choice = request_pdu[offset + 5];
else
request.service_choice = request_pdu[offset + 3];
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3))
reply.service_choice = reply_pdu[offset + 4];
else
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
if (request.invoke_id != reply.invoke_id) {
return false;
}
/* these services don't have service choice included */
if ((reply.pdu_type != PDU_TYPE_REJECT) &&
(reply.pdu_type != PDU_TYPE_ABORT) &&
(reply.pdu_type != PDU_TYPE_SEGMENT_ACK)) {
if (request.service_choice != reply.service_choice) {
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
return false;
}
return true;
}
/* MS/TP Frame Format */
/* All frames are of the following format: */
/* */
@@ -1092,7 +992,7 @@ static bool MSTP_Master_Node_FSM(void)
if (!timeout) {
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
if (pkt != NULL) {
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&InputBuffer[0], DataLength, SourceAddress,
&pkt->buffer[0], pkt->length, pkt->destination_mac);
} else {
@@ -1191,7 +1091,7 @@ static void MSTP_Slave_Node_FSM(void)
} else if (MSTP_Flag.ReceivePacketPending) {
if (!Ringbuf_Empty(&PDU_Queue)) {
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
matched = dlmstp_compare_data_expecting_reply(&InputBuffer[0],
matched = npdu_is_data_expecting_reply(&InputBuffer[0],
DataLength, SourceAddress, &pkt->buffer[0], pkt->length,
pkt->destination_mac);
if (matched) {
ports/bsd/dlmstp.c
+1 -141
View File
@@ -166,146 +166,6 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
return pdu_len;
}
/**
* @brief Determine if the reply packet is the data expected
* @param request_pdu - PDU of the data
* @param request_pdu_len - number of bytes of PDU data
* @param src_address - source address of the request
* @param reply_pdu - PDU of the data
* @param reply_pdu_len - number of bytes of PDU data
* @param dest_address - the destination address for this data
* @return true if the reply packet is the data expected
*/
static bool dlmstp_compare_data_expecting_reply(
const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* unused parameters */
(void)request_pdu_len;
(void)reply_pdu_len;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = (uint16_t)bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Request is Network message.\n");
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
debug_printf("DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n");
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3)) {
request.service_choice = request_pdu[offset + 5];
} else {
request.service_choice = request_pdu[offset + 3];
}
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = (uint16_t)bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Reply is Network message.\n");
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3)) {
reply.service_choice = reply_pdu[offset + 4];
} else {
reply.service_choice = reply_pdu[offset + 2];
}
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
/* these don't have service choice included */
if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT) ||
(reply.pdu_type == PDU_TYPE_SEGMENT_ACK)) {
if (request.invoke_id != reply.invoke_id) {
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
} else {
if (request.invoke_id != reply.invoke_id) {
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
if (request.service_choice != reply.service_choice) {
debug_printf("DLMSTP: DER Compare failed: "
"Service choice mismatch.\n");
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n");
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Priority mismatch.\n");
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
debug_printf("DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n");
return false;
}
return true;
}
/**
* @brief The MS/TP state machine uses this function for getting data to send
* as the reply to a DATA_EXPECTING_REPLY frame, or nothing
@@ -326,7 +186,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
}
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
ports/bsd/dlmstp_port.c
+2 -132
View File
@@ -447,136 +447,6 @@ void MSTP_Send_Frame(
RS485_Send_Frame(mstp_port, buffer, nbytes);
}
static bool dlmstp_compare_data_expecting_reply(
const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* unused parameters */
(void)request_pdu_len;
(void)reply_pdu_len;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Request is Network message.\n");
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
debug_printf("DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n");
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3)) {
request.service_choice = request_pdu[offset + 5];
} else {
request.service_choice = request_pdu[offset + 3];
}
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Reply is Network message.\n");
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3)) {
reply.service_choice = reply_pdu[offset + 4];
} else {
reply.service_choice = reply_pdu[offset + 2];
}
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
/* these don't have service choice included */
if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT) ||
(reply.pdu_type == PDU_TYPE_SEGMENT_ACK)) {
if (request.invoke_id != reply.invoke_id) {
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
} else {
if (request.invoke_id != reply.invoke_id) {
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
if (request.service_choice != reply.service_choice) {
debug_printf("DLMSTP: DER Compare failed: "
"Service choice mismatch.\n");
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n");
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Priority mismatch.\n");
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
debug_printf("DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n");
return false;
}
return true;
}
/* Get the reply to a DATA_EXPECTING_REPLY frame, or nothing */
uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */
@@ -595,7 +465,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
}
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&poSharedData->PDU_Queue);
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
@@ -604,7 +474,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
while (!matched &&
(pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(
&poSharedData->PDU_Queue, (uint8_t *)pkt)) != NULL) {
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0],
pkt->length, pkt->destination_mac);
ports/linux/dlmstp.c
+1 -135
View File
@@ -173,140 +173,6 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
return pdu_len;
}
/**
* @brief Determine if the reply packet is the data expected
* @param request_pdu - PDU of the data
* @param request_pdu_len - number of bytes of PDU data
* @param src_address - source address of the request
* @param reply_pdu - PDU of the data
* @param reply_pdu_len - number of bytes of PDU data
* @param dest_address - the destination address for this data
* @return true if the reply packet is the data expected
*/
static bool dlmstp_compare_data_expecting_reply(
const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* unused parameters */
(void)request_pdu_len;
(void)reply_pdu_len;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = (uint16_t)bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Request is Network message.\n");
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
debug_printf("DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n");
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3)) {
request.service_choice = request_pdu[offset + 5];
} else {
request.service_choice = request_pdu[offset + 3];
}
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = (uint16_t)bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Reply is Network message.\n");
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3)) {
reply.service_choice = reply_pdu[offset + 4];
} else {
reply.service_choice = reply_pdu[offset + 2];
}
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
/* A queued request, just look for another */
return false;
}
if (request.invoke_id != reply.invoke_id) {
/* Normal to have multiple replies queued, just look for another */
return false;
}
/* these don't have service choice included */
if ((request.pdu_type != PDU_TYPE_REJECT) &&
(request.pdu_type != PDU_TYPE_ABORT) &&
(request.pdu_type != PDU_TYPE_SEGMENT_ACK)) {
if (request.service_choice != reply.service_choice) {
debug_printf("DLMSTP: DER Compare failed: "
"Service choice mismatch.\n");
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n");
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Priority mismatch.\n");
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
debug_printf("DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n");
return false;
}
return true;
}
/**
* Add a certain number of nanoseconds to the specified time.
*
@@ -374,7 +240,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(
&PDU_Queue, (uint8_t *)pkt)) {
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
ports/linux/dlmstp_port.c
+2 -132
View File
@@ -391,136 +391,6 @@ void MSTP_Send_Frame(
RS485_Send_Frame(mstp_port, buffer, nbytes);
}
static bool dlmstp_compare_data_expecting_reply(
const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* unused parameters */
(void)request_pdu_len;
(void)reply_pdu_len;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Request is Network message.\n");
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
debug_printf("DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n");
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3)) {
request.service_choice = request_pdu[offset + 5];
} else {
request.service_choice = request_pdu[offset + 3];
}
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
debug_printf("DLMSTP: DER Compare failed: "
"Reply is Network message.\n");
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3)) {
reply.service_choice = reply_pdu[offset + 4];
} else {
reply.service_choice = reply_pdu[offset + 2];
}
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
/* these don't have service choice included */
if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT) ||
(reply.pdu_type == PDU_TYPE_SEGMENT_ACK)) {
if (request.invoke_id != reply.invoke_id) {
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
} else {
if (request.invoke_id != reply.invoke_id) {
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
if (request.service_choice != reply.service_choice) {
debug_printf("DLMSTP: DER Compare failed: "
"Service choice mismatch.\n");
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n");
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Priority mismatch.\n");
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
debug_printf("DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n");
return false;
}
return true;
}
/* Get the reply to a DATA_EXPECTING_REPLY frame, or nothing */
uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */
@@ -539,7 +409,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
}
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&poSharedData->PDU_Queue);
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
@@ -548,7 +418,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
while (!matched &&
(pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(
&poSharedData->PDU_Queue, (uint8_t *)pkt)) != NULL) {
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0],
pkt->length, pkt->destination_mac);
ports/win32/dlmstp-mm.c
+2 -112
View File
@@ -304,116 +304,6 @@ void MSTP_Send_Frame(
RS485_Send_Frame(mstp_port, buffer, nbytes);
}
bool dlmstp_compare_data_expecting_reply(
const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
const BACNET_ADDRESS *dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* unused parameters */
request_pdu_len = request_pdu_len;
reply_pdu_len = reply_pdu_len;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3)) {
request.service_choice = request_pdu[offset + 5];
} else {
request.service_choice = request_pdu[offset + 3];
}
/* decode the reply data */
bacnet_address_copy(&reply.address, dest_address);
offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3)) {
reply.service_choice = reply_pdu[offset + 4];
} else {
reply.service_choice = reply_pdu[offset + 2];
}
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
/* these don't have service choice included */
if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT) ||
(reply.pdu_type == PDU_TYPE_SEGMENT_ACK)) {
if (request.invoke_id != reply.invoke_id) {
return false;
}
} else {
if (request.invoke_id != reply.invoke_id) {
return false;
}
if (request.service_choice != reply.service_choice) {
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
return false;
}
return true;
}
/* Get the reply to a DATA_EXPECTING_REPLY frame, or nothing */
uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */
@@ -435,10 +325,10 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
return 0;
}
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, &Transmit_Packet.pdu[0],
Transmit_Packet.pdu_len, &Transmit_Packet.address);
Transmit_Packet.pdu_len, destination);
if (!matched) {
return 0;
}
ports/win32/dlmstp.c
+1 -123
View File
@@ -156,128 +156,6 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
return pdu_len;
}
/**
* @brief Determine if the reply packet is the data expected
* @param request_pdu - PDU of the data
* @param request_pdu_len - number of bytes of PDU data
* @param src_address - source address of the request
* @param reply_pdu - PDU of the data
* @param reply_pdu_len - number of bytes of PDU data
* @param dest_address - the destination address for this data
* @return true if the reply packet is the data expected
*/
static bool dlmstp_compare_data_expecting_reply(
const uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
const uint8_t *reply_pdu,
uint16_t reply_pdu_len,
uint8_t dest_address)
{
uint16_t offset;
/* One way to check the message is to compare NPDU
src, dest, along with the APDU type, invoke id.
Seems a bit overkill */
struct DER_compare_t {
BACNET_NPDU_DATA npdu_data;
BACNET_ADDRESS address;
uint8_t pdu_type;
uint8_t invoke_id;
uint8_t service_choice;
};
struct DER_compare_t request;
struct DER_compare_t reply;
/* unused parameters */
(void)request_pdu_len;
(void)reply_pdu_len;
/* decode the request data */
request.address.mac[0] = src_address;
request.address.mac_len = 1;
offset = (uint16_t)bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
return false;
}
request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
return false;
}
request.invoke_id = request_pdu[offset + 2];
/* segmented message? */
if (request_pdu[offset] & BIT(3)) {
request.service_choice = request_pdu[offset + 5];
} else {
request.service_choice = request_pdu[offset + 3];
}
/* decode the reply data */
reply.address.mac[0] = dest_address;
reply.address.mac_len = 1;
offset = (uint16_t)bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
return false;
}
/* reply could be a lot of things:
confirmed, simple ack, abort, reject, error */
reply.pdu_type = reply_pdu[offset] & 0xF0;
switch (reply.pdu_type) {
case PDU_TYPE_SIMPLE_ACK:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_COMPLEX_ACK:
reply.invoke_id = reply_pdu[offset + 1];
/* segmented message? */
if (reply_pdu[offset] & BIT(3)) {
reply.service_choice = reply_pdu[offset + 4];
} else {
reply.service_choice = reply_pdu[offset + 2];
}
break;
case PDU_TYPE_ERROR:
reply.invoke_id = reply_pdu[offset + 1];
reply.service_choice = reply_pdu[offset + 2];
break;
case PDU_TYPE_REJECT:
case PDU_TYPE_ABORT:
case PDU_TYPE_SEGMENT_ACK:
reply.invoke_id = reply_pdu[offset + 1];
break;
default:
return false;
}
/* these don't have service choice included */
if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT) ||
(reply.pdu_type == PDU_TYPE_SEGMENT_ACK)) {
if (request.invoke_id != reply.invoke_id) {
return false;
}
} else {
if (request.invoke_id != reply.invoke_id) {
return false;
}
if (request.service_choice != reply.service_choice) {
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
return false;
}
if (request.npdu_data.priority != reply.npdu_data.priority) {
return false;
}
if (!bacnet_address_same(&request.address, &reply.address)) {
return false;
}
return true;
}
/**
* @brief The MS/TP state machine uses this function for getting data to send
* as the reply to a DATA_EXPECTING_REPLY frame, or nothing
@@ -298,7 +176,7 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
}
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(
matched = npdu_is_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);