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

Issues/issue 632 invoke id mismatch (#633)

Browse Source 
Fixed nuisance print messages in ports/linux/dlmstp by changing to debug print only.
This commit is contained in:
Steve Karg
2024-05-07 06:58:26 -05:00
committed by GitHub
parent 96222574f8
commit 06ff987b79
3 changed files with 135 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ports/linux/dlmstp_linux.c
+67 -145
View File
@@ -37,6 +37,7 @@
#include "bacnet/npdu.h"
#include "bacnet/datalink/mstp.h"
#include "bacnet/basic/sys/ringbuf.h"
#include "bacnet/basic/sys/debug.h"
/* port specific */
#include "dlmstp_linux.h"
#include "rs485.h"
@@ -129,7 +130,8 @@ void dlmstp_cleanup(void *poPort)
}
/* returns number of bytes sent on success, zero on failure */
int dlmstp_send_pdu(void *poPort,
int dlmstp_send_pdu(
void *poPort,
BACNET_ADDRESS *dest, /* destination address */
uint8_t *pdu, /* any data to be sent - may be null */
unsigned pdu_len)
@@ -164,7 +166,8 @@ int dlmstp_send_pdu(void *poPort,
return bytes_sent;
}
uint16_t dlmstp_receive(void *poPort,
uint16_t dlmstp_receive(
void *poPort,
BACNET_ADDRESS *src, /* source address */
uint8_t *pdu, /* PDU data */
uint16_t max_pdu, /* amount of space available in the PDU */
@@ -192,11 +195,13 @@ uint16_t dlmstp_receive(void *poPort,
if (poSharedData->Receive_Packet.pdu_len) {
poSharedData->MSTP_Packets++;
if (src) {
memmove(src, &poSharedData->Receive_Packet.address,
memmove(
src, &poSharedData->Receive_Packet.address,
sizeof(poSharedData->Receive_Packet.address));
}
if (pdu) {
memmove(pdu, &poSharedData->Receive_Packet.pdu,
memmove(
pdu, &poSharedData->Receive_Packet.pdu,
sizeof(poSharedData->Receive_Packet.pdu));
}
pdu_len = poSharedData->Receive_Packet.pdu_len;
@@ -229,8 +234,7 @@ void *dlmstp_receive_fsm_task(void *pArg)
(mstp_port->ReceivedInvalidFrame == false)) {
do {
RS485_Check_UART_Data(mstp_port);
MSTP_Receive_Frame_FSM(
(struct mstp_port_struct_t *)pArg);
MSTP_Receive_Frame_FSM((struct mstp_port_struct_t *)pArg);
received_frame = mstp_port->ReceivedValidFrame ||
mstp_port->ReceivedInvalidFrame;
if (received_frame) {
@@ -340,7 +344,8 @@ uint16_t MSTP_Put_Receive(struct mstp_port_struct_t *mstp_port)
pdu_len = mstp_port->DataLength;
if (pdu_len > sizeof(poSharedData->Receive_Packet.pdu))
pdu_len = sizeof(poSharedData->Receive_Packet.pdu);
memmove((void *)&poSharedData->Receive_Packet.pdu[0],
memmove(
(void *)&poSharedData->Receive_Packet.pdu[0],
(void *)&mstp_port->InputBuffer[0], pdu_len);
dlmstp_fill_bacnet_address(
&poSharedData->Receive_Packet.address, mstp_port->SourceAddress);
@@ -354,8 +359,7 @@ uint16_t MSTP_Put_Receive(struct mstp_port_struct_t *mstp_port)
/* for the MS/TP state machine to use for getting data to send */
/* Return: amount of PDU data */
uint16_t MSTP_Get_Send(
struct mstp_port_struct_t *mstp_port, unsigned timeout)
uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */
uint16_t pdu_len = 0;
uint8_t frame_type = 0;
@@ -377,10 +381,10 @@ uint16_t MSTP_Get_Send(
frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
}
/* convert the PDU into the MSTP Frame */
pdu_len =
MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
pdu_len = MSTP_Create_Frame(
&mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
(void)Ringbuf_Pop(&poSharedData->PDU_Queue, NULL);
return pdu_len;
@@ -393,14 +397,13 @@ uint16_t MSTP_Get_Send(
* @param nbytes - number of bytes of data to send
*/
void MSTP_Send_Frame(
struct mstp_port_struct_t *mstp_port,
uint8_t * buffer,
uint16_t nbytes)
struct mstp_port_struct_t *mstp_port, uint8_t *buffer, uint16_t nbytes)
{
RS485_Send_Frame(mstp_port, buffer, nbytes);
}
bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
bool dlmstp_compare_data_expecting_reply(
uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
uint8_t *reply_pdu,
@@ -427,23 +430,18 @@ bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
/* 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);
offset = bacnet_npdu_decode(
request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Request is Network message.\n");
#endif
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) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n");
return false;
}
request.invoke_id = request_pdu[offset + 2];
@@ -459,11 +457,8 @@ bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Reply is Network message.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"Reply is Network message.\n");
return false;
}
/* reply could be a lot of things:
@@ -498,57 +493,40 @@ bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT)) {
if (request.invoke_id != reply.invoke_id) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
} else {
if (request.invoke_id != reply.invoke_id) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n");
return false;
}
if (request.service_choice != reply.service_choice) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Service choice mismatch.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"Service choice mismatch.\n");
return false;
}
}
if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n");
return false;
}
#if 0
/* the NDPU priority doesn't get passed through the stack, and
all outgoing messages have NORMAL priority */
if (request.npdu_data.priority != reply.npdu_data.priority) {
#if PRINT_ENABLED
fprintf(stderr,
debug_printf(
"DLMSTP: DER Compare failed: " "NPDU Priority mismatch.\n");
#endif
return false;
}
#endif
if (!bacnet_address_same(&request.address, &reply.address)) {
#if PRINT_ENABLED
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n");
#endif
debug_printf("DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n");
return false;
}
@@ -556,8 +534,7 @@ bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
}
/* 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)
uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */
uint16_t pdu_len = 0; /* return value */
bool matched = false;
@@ -574,14 +551,15 @@ uint16_t MSTP_Get_Reply(
}
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&poSharedData->PDU_Queue);
/* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(&mstp_port->InputBuffer[0],
mstp_port->DataLength, mstp_port->SourceAddress,
(uint8_t *)&pkt->buffer[0], pkt->length, pkt->destination_mac);
matched = dlmstp_compare_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
if (!matched) {
/* Walk the rest of the ring buffer to see if we can find a match */
while (!matched &&
(pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(
&poSharedData->PDU_Queue, (uint8_t *)pkt)) != NULL) {
(pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(
&poSharedData->PDU_Queue, (uint8_t *)pkt)) != NULL) {
matched = dlmstp_compare_data_expecting_reply(
&mstp_port->InputBuffer[0], mstp_port->DataLength,
mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0],
@@ -598,10 +576,10 @@ uint16_t MSTP_Get_Reply(
frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
}
/* convert the PDU into the MSTP Frame */
pdu_len =
MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
pdu_len = MSTP_Create_Frame(
&mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
/* This will pop the element no matter where we found it */
(void)Ringbuf_Pop_Element(&poSharedData->PDU_Queue, (uint8_t *)pkt, NULL);
@@ -610,26 +588,13 @@ uint16_t MSTP_Get_Reply(
void dlmstp_set_mac_address(void *poPort, uint8_t mac_address)
{
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) {
return;
}
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return;
}
*/
/* Master Nodes can only have address 0-127 */
if (mac_address <= 127) {
mstp_port->This_Station = mac_address;
/* FIXME: implement your data storage */
/* I2C_Write_Byte(
EEPROM_DEVICE_ADDRESS,
mac_address,
EEPROM_MSTP_MAC_ADDR); */
if (mac_address > mstp_port->Nmax_master)
dlmstp_set_max_master(mstp_port, mac_address);
}
@@ -639,17 +604,10 @@ void dlmstp_set_mac_address(void *poPort, uint8_t mac_address)
uint8_t dlmstp_mac_address(void *poPort)
{
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) {
return 0;
}
/* poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return 0;
}
*/
return mstp_port->This_Station;
}
@@ -663,25 +621,13 @@ uint8_t dlmstp_mac_address(void *poPort)
/* node, its value shall be 1. */
void dlmstp_set_max_info_frames(void *poPort, uint8_t max_info_frames)
{
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) {
return;
}
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return;
}
*/
if (max_info_frames >= 1) {
mstp_port->Nmax_info_frames = max_info_frames;
/* FIXME: implement your data storage */
/* I2C_Write_Byte(
EEPROM_DEVICE_ADDRESS,
(uint8_t)max_info_frames,
EEPROM_MSTP_MAX_INFO_FRAMES_ADDR); */
}
return;
@@ -689,18 +635,11 @@ void dlmstp_set_max_info_frames(void *poPort, uint8_t max_info_frames)
uint8_t dlmstp_max_info_frames(void *poPort)
{
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) {
return 0;
}
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return 0;
}
*/
return mstp_port->Nmax_info_frames;
}
@@ -711,26 +650,13 @@ uint8_t dlmstp_max_info_frames(void *poPort)
/* its value shall be 127. */
void dlmstp_set_max_master(void *poPort, uint8_t max_master)
{
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) {
return;
}
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return;
}
*/
if (max_master <= 127) {
if (mstp_port->This_Station <= max_master) {
mstp_port->Nmax_master = max_master;
/* FIXME: implement your data storage */
/* I2C_Write_Byte(
EEPROM_DEVICE_ADDRESS,
max_master,
EEPROM_MSTP_MAX_MASTER_ADDR); */
}
}
@@ -739,18 +665,12 @@ void dlmstp_set_max_master(void *poPort, uint8_t max_master)
uint8_t dlmstp_max_master(void *poPort)
{
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) {
return 0;
}
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return 0;
}
*/
return mstp_port->Nmax_master;
}
@@ -873,14 +793,16 @@ bool dlmstp_init(void *poPort, char *ifname)
poSharedData->RS485_Port_Name = ifname;
/* initialize PDU queue */
Ringbuf_Init(&poSharedData->PDU_Queue, (uint8_t *)&poSharedData->PDU_Buffer,
Ringbuf_Init(
&poSharedData->PDU_Queue, (uint8_t *)&poSharedData->PDU_Buffer,
sizeof(struct mstp_pdu_packet), MSTP_PDU_PACKET_COUNT);
/* initialize packet queue */
poSharedData->Receive_Packet.ready = false;
poSharedData->Receive_Packet.pdu_len = 0;
rv = sem_init(&poSharedData->Receive_Packet_Flag, 0, 0);
if (rv != 0) {
fprintf(stderr,
fprintf(
stderr,
"MS/TP Interface: %s\n cannot allocate PThread Condition.\n",
ifname);
exit(1);
@@ -892,7 +814,8 @@ bool dlmstp_init(void *poPort, char *ifname)
Open device for reading and writing.
Blocking mode - more CPU effecient
*/
poSharedData->RS485_Handle = open(poSharedData->RS485_Port_Name,
poSharedData->RS485_Handle = open(
poSharedData->RS485_Port_Name,
O_RDWR | O_NOCTTY | O_NONBLOCK /*| O_NDELAY */);
if (poSharedData->RS485_Handle < 0) {
perror(poSharedData->RS485_Port_Name);
@@ -930,7 +853,8 @@ bool dlmstp_init(void *poPort, char *ifname)
usleep(200000);
tcflush(poSharedData->RS485_Handle, TCIOFLUSH);
/* ringbuffer */
FIFO_Init(&poSharedData->Rx_FIFO, poSharedData->Rx_Buffer,
FIFO_Init(
&poSharedData->Rx_FIFO, poSharedData->Rx_Buffer,
sizeof(poSharedData->Rx_Buffer));
printf("=success!\n");
mstp_port->InputBuffer = &poSharedData->RxBuffer[0];
@@ -941,12 +865,10 @@ bool dlmstp_init(void *poPort, char *ifname)
mstp_port->SilenceTimer = Timer_Silence;
mstp_port->SilenceTimerReset = Timer_Silence_Reset;
MSTP_Init(mstp_port);
#if PRINT_ENABLED
fprintf(stderr, "MS/TP MAC: %02X\n", mstp_port->This_Station);
fprintf(stderr, "MS/TP Max_Master: %02X\n", mstp_port->Nmax_master);
fprintf(stderr, "MS/TP Max_Info_Frames: %u\n", mstp_port->Nmax_info_frames);
#endif
debug_fprintf(stderr, "MS/TP MAC: %02X\n", mstp_port->This_Station);
debug_fprintf(stderr, "MS/TP Max_Master: %02X\n", mstp_port->Nmax_master);
debug_fprintf(
stderr, "MS/TP Max_Info_Frames: %u\n", mstp_port->Nmax_info_frames);
rv = pthread_create(&hThread, NULL, dlmstp_master_fsm_task, mstp_port);
if (rv != 0) {
fprintf(stderr, "Failed to start Master Node FSM task\n");