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.c
+67 -115
View File
@@ -53,29 +53,17 @@ uint16_t MSTP_Packets = 0;
/* packet queues */
static DLMSTP_PACKET Receive_Packet;
/* mechanism to wait for a packet */
/*
static RT_COND Receive_Packet_Flag;
static RT_MUTEX Receive_Packet_Mutex;
*/
static pthread_cond_t Receive_Packet_Flag;
static pthread_mutex_t Receive_Packet_Mutex;
/* mechanism to wait for a frame in state machine */
/*
static RT_COND Received_Frame_Flag;
static RT_MUTEX Received_Frame_Mutex;
*/
static pthread_cond_t Received_Frame_Flag;
static pthread_mutex_t Received_Frame_Mutex;
static pthread_cond_t Master_Done_Flag;
static pthread_mutex_t Master_Done_Mutex;
static pthread_mutex_t Ring_Buffer_Mutex;
static pthread_mutex_t Thread_Mutex;
static pthread_t hThread;
static bool run_thread;
/*RT_TASK Receive_Task, Fsm_Task;*/
/* local MS/TP port data - shared with RS-485 */
static struct mstp_port_struct_t MSTP_Port;
/* buffers needed by mstp port struct */
@@ -184,7 +172,8 @@ static void get_abstime(struct timespec *abstime, unsigned long milliseconds)
{
clock_gettime(CLOCK_MONOTONIC, abstime);
if (milliseconds > 1000) {
fprintf(stderr, "DLMSTP: limited timeout of %lums to 1000ms\n",
fprintf(
stderr, "DLMSTP: limited timeout of %lums to 1000ms\n",
milliseconds);
milliseconds = 1000;
}
@@ -207,7 +196,8 @@ void dlmstp_cleanup(void)
}
/* returns number of bytes sent on success, zero on failure */
int dlmstp_send_pdu(BACNET_ADDRESS *dest, /* destination address */
int dlmstp_send_pdu(
BACNET_ADDRESS *dest, /* destination address */
BACNET_NPDU_DATA *npdu_data, /* network information */
uint8_t *pdu, /* any data to be sent - may be null */
unsigned pdu_len)
@@ -238,7 +228,8 @@ int dlmstp_send_pdu(BACNET_ADDRESS *dest, /* destination address */
return bytes_sent;
}
uint16_t dlmstp_receive(BACNET_ADDRESS *src, /* source address */
uint16_t dlmstp_receive(
BACNET_ADDRESS *src, /* source address */
uint8_t *pdu, /* PDU data */
uint16_t max_pdu, /* amount of space available in the PDU */
unsigned timeout)
@@ -257,7 +248,8 @@ uint16_t dlmstp_receive(BACNET_ADDRESS *src, /* source address */
if (Receive_Packet.pdu_len) {
MSTP_Packets++;
if (src) {
memmove(src, &Receive_Packet.address,
memmove(
src, &Receive_Packet.address,
sizeof(Receive_Packet.address));
}
if (pdu) {
@@ -371,8 +363,9 @@ uint16_t MSTP_Put_Receive(struct mstp_port_struct_t *mstp_port)
if (pdu_len == 0) {
debug_printf("MS/TP: PDU Length is 0!\n");
}
memmove((void *)&Receive_Packet.pdu[0],
(void *)&mstp_port->InputBuffer[0], pdu_len);
memmove(
(void *)&Receive_Packet.pdu[0], (void *)&mstp_port->InputBuffer[0],
pdu_len);
dlmstp_fill_bacnet_address(
&Receive_Packet.address, mstp_port->SourceAddress);
Receive_Packet.pdu_len = mstp_port->DataLength;
@@ -405,10 +398,10 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
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(&PDU_Queue, NULL);
pthread_mutex_unlock(&Ring_Buffer_Mutex);
@@ -422,14 +415,13 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
* @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);
}
static bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
static bool dlmstp_compare_data_expecting_reply(
uint8_t *request_pdu,
uint16_t request_pdu_len,
uint8_t src_address,
uint8_t *reply_pdu,
@@ -456,23 +448,18 @@ static 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];
@@ -488,11 +475,8 @@ static 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:
@@ -527,57 +511,40 @@ static 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;
}
@@ -598,9 +565,10 @@ 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(&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) {
return 0;
}
@@ -610,10 +578,10 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
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(&PDU_Queue, NULL);
return pdu_len;
@@ -624,11 +592,6 @@ void dlmstp_set_mac_address(uint8_t mac_address)
/* 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(mac_address);
}
@@ -652,11 +615,6 @@ void dlmstp_set_max_info_frames(uint8_t max_info_frames)
{
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;
@@ -677,11 +635,6 @@ void dlmstp_set_max_master(uint8_t max_master)
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); */
}
}
@@ -743,7 +696,8 @@ bool dlmstp_init(char *ifname)
pthread_condattr_init(&attr);
if ((rv = pthread_condattr_setclock(&attr, CLOCK_MONOTONIC)) != 0) {
fprintf(stderr, "MS/TP Interface: %s\n failed to set MONOTONIC clock\n",
fprintf(
stderr, "MS/TP Interface: %s\n failed to set MONOTONIC clock\n",
ifname);
exit(1);
}
@@ -752,30 +706,31 @@ bool dlmstp_init(char *ifname)
pthread_mutex_init(&Thread_Mutex, NULL);
/* initialize PDU queue */
Ringbuf_Init(&PDU_Queue, (uint8_t *)&PDU_Buffer,
sizeof(struct mstp_pdu_packet), MSTP_PDU_PACKET_COUNT);
Ringbuf_Init(
&PDU_Queue, (uint8_t *)&PDU_Buffer, sizeof(struct mstp_pdu_packet),
MSTP_PDU_PACKET_COUNT);
/* initialize packet queue */
Receive_Packet.ready = false;
Receive_Packet.pdu_len = 0;
rv = pthread_cond_init(&Receive_Packet_Flag, &attr);
if (rv != 0) {
fprintf(stderr,
fprintf(
stderr,
"MS/TP Interface: %s\n cannot allocate PThread Condition.\n",
ifname);
exit(1);
}
rv = pthread_mutex_init(&Receive_Packet_Mutex, NULL);
if (rv != 0) {
fprintf(stderr,
"MS/TP Interface: %s\n cannot allocate PThread Mutex.\n", ifname);
fprintf(
stderr, "MS/TP Interface: %s\n cannot allocate PThread Mutex.\n",
ifname);
exit(1);
}
/* initialize hardware */
if (ifname) {
RS485_Set_Interface(ifname);
#if PRINT_ENABLED
fprintf(stderr, "MS/TP Interface: %s\n", ifname);
#endif
debug_fprintf(stderr, "MS/TP Interface: %s\n", ifname);
}
RS485_Initialize();
MSTP_Port.InputBuffer = &RxBuffer[0];
@@ -786,17 +741,12 @@ bool dlmstp_init(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);
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);
fflush(stderr);
#endif
/* start the threads */
/* rv = pthread_create(&hThread, NULL, dlmstp_receive_fsm_task, NULL); */
/* if (rv != 0) {
fprintf(stderr, "Failed to start recive FSM task\n");
} */
/* start one thread */
run_thread = true;
rv = pthread_create(&hThread, NULL, dlmstp_master_fsm_task, NULL);
if (rv != 0) {
@@ -809,7 +759,8 @@ bool dlmstp_init(char *ifname)
#ifdef TEST_DLMSTP
#include <stdio.h>
void apdu_handler(BACNET_ADDRESS *src, /* source address */
void apdu_handler(
BACNET_ADDRESS *src, /* source address */
uint8_t *apdu, /* APDU data */
uint16_t pdu_len)
{ /* for confirmed messages */
@@ -836,8 +787,9 @@ int main(int argc, char *argv[])
/* forever task */
for (;;) {
pdu_len = dlmstp_receive(NULL, NULL, 0, UINT_MAX);
MSTP_Create_And_Send_Frame(&MSTP_Port, FRAME_TYPE_TEST_REQUEST,
MSTP_Port.SourceAddress, MSTP_Port.This_Station, NULL, 0);
MSTP_Create_And_Send_Frame(
&MSTP_Port, FRAME_TYPE_TEST_REQUEST, MSTP_Port.SourceAddress,
MSTP_Port.This_Station, NULL, 0);
}
return 0;
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");
src/bacnet/basic/sys/debug.c
+1
View File
@@ -203,6 +203,7 @@ int debug_fprintf(FILE *stream, const char *format, ...)
va_end(ap);
fflush(stream);
#else
(void)stream;
(void)format;
#endif
return length;