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
+55 -103
View File
@@ -53,29 +53,17 @@ uint16_t MSTP_Packets = 0;
/* packet queues */ /* packet queues */
static DLMSTP_PACKET Receive_Packet; static DLMSTP_PACKET Receive_Packet;
/* mechanism to wait for a 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_cond_t Receive_Packet_Flag;
static pthread_mutex_t Receive_Packet_Mutex; 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_cond_t Received_Frame_Flag;
static pthread_mutex_t Received_Frame_Mutex; static pthread_mutex_t Received_Frame_Mutex;
static pthread_cond_t Master_Done_Flag; static pthread_cond_t Master_Done_Flag;
static pthread_mutex_t Master_Done_Mutex; static pthread_mutex_t Master_Done_Mutex;
static pthread_mutex_t Ring_Buffer_Mutex; static pthread_mutex_t Ring_Buffer_Mutex;
static pthread_mutex_t Thread_Mutex; static pthread_mutex_t Thread_Mutex;
static pthread_t hThread; static pthread_t hThread;
static bool run_thread; static bool run_thread;
/*RT_TASK Receive_Task, Fsm_Task;*/
/* local MS/TP port data - shared with RS-485 */ /* local MS/TP port data - shared with RS-485 */
static struct mstp_port_struct_t MSTP_Port; static struct mstp_port_struct_t MSTP_Port;
/* buffers needed by mstp port struct */ /* 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); clock_gettime(CLOCK_MONOTONIC, abstime);
if (milliseconds > 1000) { 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);
milliseconds = 1000; milliseconds = 1000;
} }
@@ -207,7 +196,8 @@ void dlmstp_cleanup(void)
} }
/* returns number of bytes sent on success, zero on failure */ /* 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 */ BACNET_NPDU_DATA *npdu_data, /* network information */
uint8_t *pdu, /* any data to be sent - may be null */ uint8_t *pdu, /* any data to be sent - may be null */
unsigned pdu_len) unsigned pdu_len)
@@ -238,7 +228,8 @@ int dlmstp_send_pdu(BACNET_ADDRESS *dest, /* destination address */
return bytes_sent; 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 */ uint8_t *pdu, /* PDU data */
uint16_t max_pdu, /* amount of space available in the PDU */ uint16_t max_pdu, /* amount of space available in the PDU */
unsigned timeout) unsigned timeout)
@@ -257,7 +248,8 @@ uint16_t dlmstp_receive(BACNET_ADDRESS *src, /* source address */
if (Receive_Packet.pdu_len) { if (Receive_Packet.pdu_len) {
MSTP_Packets++; MSTP_Packets++;
if (src) { if (src) {
memmove(src, &Receive_Packet.address, memmove(
src, &Receive_Packet.address,
sizeof(Receive_Packet.address)); sizeof(Receive_Packet.address));
} }
if (pdu) { if (pdu) {
@@ -371,8 +363,9 @@ uint16_t MSTP_Put_Receive(struct mstp_port_struct_t *mstp_port)
if (pdu_len == 0) { if (pdu_len == 0) {
debug_printf("MS/TP: PDU Length is 0!\n"); debug_printf("MS/TP: PDU Length is 0!\n");
} }
memmove((void *)&Receive_Packet.pdu[0], memmove(
(void *)&mstp_port->InputBuffer[0], pdu_len); (void *)&Receive_Packet.pdu[0], (void *)&mstp_port->InputBuffer[0],
pdu_len);
dlmstp_fill_bacnet_address( dlmstp_fill_bacnet_address(
&Receive_Packet.address, mstp_port->SourceAddress); &Receive_Packet.address, mstp_port->SourceAddress);
Receive_Packet.pdu_len = mstp_port->DataLength; Receive_Packet.pdu_len = mstp_port->DataLength;
@@ -405,8 +398,8 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY; frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
} }
/* convert the PDU into the MSTP Frame */ /* convert the PDU into the MSTP Frame */
pdu_len = pdu_len = MSTP_Create_Frame(
MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */ &mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac, mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length); mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
(void)Ringbuf_Pop(&PDU_Queue, NULL); (void)Ringbuf_Pop(&PDU_Queue, NULL);
@@ -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 * @param nbytes - number of bytes of data to send
*/ */
void MSTP_Send_Frame( void MSTP_Send_Frame(
struct mstp_port_struct_t *mstp_port, struct mstp_port_struct_t *mstp_port, uint8_t *buffer, uint16_t nbytes)
uint8_t * buffer,
uint16_t nbytes)
{ {
RS485_Send_Frame(mstp_port, buffer, 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, uint16_t request_pdu_len,
uint8_t src_address, uint8_t src_address,
uint8_t *reply_pdu, uint8_t *reply_pdu,
@@ -456,23 +448,18 @@ static bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
/* decode the request data */ /* decode the request data */
request.address.mac[0] = src_address; request.address.mac[0] = src_address;
request.address.mac_len = 1; request.address.mac_len = 1;
offset = bacnet_npdu_decode(request_pdu, request_pdu_len, NULL, offset = bacnet_npdu_decode(
&request.address, &request.npdu_data); request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) { if (request.npdu_data.network_layer_message) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Request is Network message.\n"); "Request is Network message.\n");
#endif
return false; return false;
} }
request.pdu_type = request_pdu[offset] & 0xF0; request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) { if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n"); "Not Confirmed Request.\n");
#endif
return false; return false;
} }
request.invoke_id = request_pdu[offset + 2]; 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( offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data); reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) { if (reply.npdu_data.network_layer_message) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Reply is Network message.\n"); "Reply is Network message.\n");
#endif
return false; return false;
} }
/* reply could be a lot of things: /* 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) || if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT)) { (reply.pdu_type == PDU_TYPE_ABORT)) {
if (request.invoke_id != reply.invoke_id) { if (request.invoke_id != reply.invoke_id) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n"); "Invoke ID mismatch.\n");
#endif
return false; return false;
} }
} else { } else {
if (request.invoke_id != reply.invoke_id) { if (request.invoke_id != reply.invoke_id) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n"); "Invoke ID mismatch.\n");
#endif
return false; return false;
} }
if (request.service_choice != reply.service_choice) { if (request.service_choice != reply.service_choice) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Service choice mismatch.\n"); "Service choice mismatch.\n");
#endif
return false; return false;
} }
} }
if (request.npdu_data.protocol_version != if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) { reply.npdu_data.protocol_version) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n"); "NPDU Protocol Version mismatch.\n");
#endif
return false; return false;
} }
#if 0 #if 0
/* the NDPU priority doesn't get passed through the stack, and /* the NDPU priority doesn't get passed through the stack, and
all outgoing messages have NORMAL priority */ all outgoing messages have NORMAL priority */
if (request.npdu_data.priority != reply.npdu_data.priority) { if (request.npdu_data.priority != reply.npdu_data.priority) {
#if PRINT_ENABLED debug_printf(
fprintf(stderr,
"DLMSTP: DER Compare failed: " "NPDU Priority mismatch.\n"); "DLMSTP: DER Compare failed: " "NPDU Priority mismatch.\n");
#endif
return false; return false;
} }
#endif #endif
if (!bacnet_address_same(&request.address, &reply.address)) { if (!bacnet_address_same(&request.address, &reply.address)) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n"); "BACnet Address mismatch.\n");
#endif
return false; 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); pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
/* is this the reply to the DER? */ /* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(&mstp_port->InputBuffer[0], matched = dlmstp_compare_data_expecting_reply(
mstp_port->DataLength, mstp_port->SourceAddress, &mstp_port->InputBuffer[0], mstp_port->DataLength,
(uint8_t *)&pkt->buffer[0], pkt->length, pkt->destination_mac); mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
if (!matched) { if (!matched) {
return 0; return 0;
} }
@@ -610,8 +578,8 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY; frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
} }
/* convert the PDU into the MSTP Frame */ /* convert the PDU into the MSTP Frame */
pdu_len = pdu_len = MSTP_Create_Frame(
MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */ &mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac, mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length); mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
(void)Ringbuf_Pop(&PDU_Queue, NULL); (void)Ringbuf_Pop(&PDU_Queue, NULL);
@@ -624,11 +592,6 @@ void dlmstp_set_mac_address(uint8_t mac_address)
/* Master Nodes can only have address 0-127 */ /* Master Nodes can only have address 0-127 */
if (mac_address <= 127) { if (mac_address <= 127) {
MSTP_Port.This_Station = mac_address; 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) if (mac_address > MSTP_Port.Nmax_master)
dlmstp_set_max_master(mac_address); 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) { if (max_info_frames >= 1) {
MSTP_Port.Nmax_info_frames = max_info_frames; 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; return;
@@ -677,11 +635,6 @@ void dlmstp_set_max_master(uint8_t max_master)
if (max_master <= 127) { if (max_master <= 127) {
if (MSTP_Port.This_Station <= max_master) { if (MSTP_Port.This_Station <= max_master) {
MSTP_Port.Nmax_master = 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); pthread_condattr_init(&attr);
if ((rv = pthread_condattr_setclock(&attr, CLOCK_MONOTONIC)) != 0) { 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); ifname);
exit(1); exit(1);
} }
@@ -752,30 +706,31 @@ bool dlmstp_init(char *ifname)
pthread_mutex_init(&Thread_Mutex, NULL); pthread_mutex_init(&Thread_Mutex, NULL);
/* initialize PDU queue */ /* initialize PDU queue */
Ringbuf_Init(&PDU_Queue, (uint8_t *)&PDU_Buffer, Ringbuf_Init(
sizeof(struct mstp_pdu_packet), MSTP_PDU_PACKET_COUNT); &PDU_Queue, (uint8_t *)&PDU_Buffer, sizeof(struct mstp_pdu_packet),
MSTP_PDU_PACKET_COUNT);
/* initialize packet queue */ /* initialize packet queue */
Receive_Packet.ready = false; Receive_Packet.ready = false;
Receive_Packet.pdu_len = 0; Receive_Packet.pdu_len = 0;
rv = pthread_cond_init(&Receive_Packet_Flag, &attr); rv = pthread_cond_init(&Receive_Packet_Flag, &attr);
if (rv != 0) { if (rv != 0) {
fprintf(stderr, fprintf(
stderr,
"MS/TP Interface: %s\n cannot allocate PThread Condition.\n", "MS/TP Interface: %s\n cannot allocate PThread Condition.\n",
ifname); ifname);
exit(1); exit(1);
} }
rv = pthread_mutex_init(&Receive_Packet_Mutex, NULL); rv = pthread_mutex_init(&Receive_Packet_Mutex, NULL);
if (rv != 0) { if (rv != 0) {
fprintf(stderr, fprintf(
"MS/TP Interface: %s\n cannot allocate PThread Mutex.\n", ifname); stderr, "MS/TP Interface: %s\n cannot allocate PThread Mutex.\n",
ifname);
exit(1); exit(1);
} }
/* initialize hardware */ /* initialize hardware */
if (ifname) { if (ifname) {
RS485_Set_Interface(ifname); RS485_Set_Interface(ifname);
#if PRINT_ENABLED debug_fprintf(stderr, "MS/TP Interface: %s\n", ifname);
fprintf(stderr, "MS/TP Interface: %s\n", ifname);
#endif
} }
RS485_Initialize(); RS485_Initialize();
MSTP_Port.InputBuffer = &RxBuffer[0]; MSTP_Port.InputBuffer = &RxBuffer[0];
@@ -786,17 +741,12 @@ bool dlmstp_init(char *ifname)
MSTP_Port.SilenceTimer = Timer_Silence; MSTP_Port.SilenceTimer = Timer_Silence;
MSTP_Port.SilenceTimerReset = Timer_Silence_Reset; MSTP_Port.SilenceTimerReset = Timer_Silence_Reset;
MSTP_Init(&MSTP_Port); MSTP_Init(&MSTP_Port);
#if PRINT_ENABLED debug_fprintf(stderr, "MS/TP MAC: %02X\n", MSTP_Port.This_Station);
fprintf(stderr, "MS/TP MAC: %02X\n", MSTP_Port.This_Station); debug_fprintf(stderr, "MS/TP Max_Master: %02X\n", MSTP_Port.Nmax_master);
fprintf(stderr, "MS/TP Max_Master: %02X\n", MSTP_Port.Nmax_master); debug_fprintf(
fprintf(stderr, "MS/TP Max_Info_Frames: %u\n", MSTP_Port.Nmax_info_frames); stderr, "MS/TP Max_Info_Frames: %u\n", MSTP_Port.Nmax_info_frames);
fflush(stderr); fflush(stderr);
#endif /* start one thread */
/* 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");
} */
run_thread = true; run_thread = true;
rv = pthread_create(&hThread, NULL, dlmstp_master_fsm_task, NULL); rv = pthread_create(&hThread, NULL, dlmstp_master_fsm_task, NULL);
if (rv != 0) { if (rv != 0) {
@@ -809,7 +759,8 @@ bool dlmstp_init(char *ifname)
#ifdef TEST_DLMSTP #ifdef TEST_DLMSTP
#include <stdio.h> #include <stdio.h>
void apdu_handler(BACNET_ADDRESS *src, /* source address */ void apdu_handler(
BACNET_ADDRESS *src, /* source address */
uint8_t *apdu, /* APDU data */ uint8_t *apdu, /* APDU data */
uint16_t pdu_len) uint16_t pdu_len)
{ /* for confirmed messages */ { /* for confirmed messages */
@@ -836,8 +787,9 @@ int main(int argc, char *argv[])
/* forever task */ /* forever task */
for (;;) { for (;;) {
pdu_len = dlmstp_receive(NULL, NULL, 0, UINT_MAX); pdu_len = dlmstp_receive(NULL, NULL, 0, UINT_MAX);
MSTP_Create_And_Send_Frame(&MSTP_Port, FRAME_TYPE_TEST_REQUEST, MSTP_Create_And_Send_Frame(
MSTP_Port.SourceAddress, MSTP_Port.This_Station, NULL, 0); &MSTP_Port, FRAME_TYPE_TEST_REQUEST, MSTP_Port.SourceAddress,
MSTP_Port.This_Station, NULL, 0);
} }
return 0; return 0;
ports/linux/dlmstp_linux.c
+53 -131
View File
@@ -37,6 +37,7 @@
#include "bacnet/npdu.h" #include "bacnet/npdu.h"
#include "bacnet/datalink/mstp.h" #include "bacnet/datalink/mstp.h"
#include "bacnet/basic/sys/ringbuf.h" #include "bacnet/basic/sys/ringbuf.h"
#include "bacnet/basic/sys/debug.h"
/* port specific */ /* port specific */
#include "dlmstp_linux.h" #include "dlmstp_linux.h"
#include "rs485.h" #include "rs485.h"
@@ -129,7 +130,8 @@ void dlmstp_cleanup(void *poPort)
} }
/* returns number of bytes sent on success, zero on failure */ /* 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 */ BACNET_ADDRESS *dest, /* destination address */
uint8_t *pdu, /* any data to be sent - may be null */ uint8_t *pdu, /* any data to be sent - may be null */
unsigned pdu_len) unsigned pdu_len)
@@ -164,7 +166,8 @@ int dlmstp_send_pdu(void *poPort,
return bytes_sent; return bytes_sent;
} }
uint16_t dlmstp_receive(void *poPort, uint16_t dlmstp_receive(
void *poPort,
BACNET_ADDRESS *src, /* source address */ BACNET_ADDRESS *src, /* source address */
uint8_t *pdu, /* PDU data */ uint8_t *pdu, /* PDU data */
uint16_t max_pdu, /* amount of space available in the PDU */ 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) { if (poSharedData->Receive_Packet.pdu_len) {
poSharedData->MSTP_Packets++; poSharedData->MSTP_Packets++;
if (src) { if (src) {
memmove(src, &poSharedData->Receive_Packet.address, memmove(
src, &poSharedData->Receive_Packet.address,
sizeof(poSharedData->Receive_Packet.address)); sizeof(poSharedData->Receive_Packet.address));
} }
if (pdu) { if (pdu) {
memmove(pdu, &poSharedData->Receive_Packet.pdu, memmove(
pdu, &poSharedData->Receive_Packet.pdu,
sizeof(poSharedData->Receive_Packet.pdu)); sizeof(poSharedData->Receive_Packet.pdu));
} }
pdu_len = poSharedData->Receive_Packet.pdu_len; pdu_len = poSharedData->Receive_Packet.pdu_len;
@@ -229,8 +234,7 @@ void *dlmstp_receive_fsm_task(void *pArg)
(mstp_port->ReceivedInvalidFrame == false)) { (mstp_port->ReceivedInvalidFrame == false)) {
do { do {
RS485_Check_UART_Data(mstp_port); RS485_Check_UART_Data(mstp_port);
MSTP_Receive_Frame_FSM( MSTP_Receive_Frame_FSM((struct mstp_port_struct_t *)pArg);
(struct mstp_port_struct_t *)pArg);
received_frame = mstp_port->ReceivedValidFrame || received_frame = mstp_port->ReceivedValidFrame ||
mstp_port->ReceivedInvalidFrame; mstp_port->ReceivedInvalidFrame;
if (received_frame) { if (received_frame) {
@@ -340,7 +344,8 @@ uint16_t MSTP_Put_Receive(struct mstp_port_struct_t *mstp_port)
pdu_len = mstp_port->DataLength; pdu_len = mstp_port->DataLength;
if (pdu_len > sizeof(poSharedData->Receive_Packet.pdu)) if (pdu_len > sizeof(poSharedData->Receive_Packet.pdu))
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); (void *)&mstp_port->InputBuffer[0], pdu_len);
dlmstp_fill_bacnet_address( dlmstp_fill_bacnet_address(
&poSharedData->Receive_Packet.address, mstp_port->SourceAddress); &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 */ /* for the MS/TP state machine to use for getting data to send */
/* Return: amount of PDU data */ /* Return: amount of PDU data */
uint16_t MSTP_Get_Send( uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */ { /* milliseconds to wait for a packet */
uint16_t pdu_len = 0; uint16_t pdu_len = 0;
uint8_t frame_type = 0; uint8_t frame_type = 0;
@@ -377,8 +381,8 @@ uint16_t MSTP_Get_Send(
frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY; frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
} }
/* convert the PDU into the MSTP Frame */ /* convert the PDU into the MSTP Frame */
pdu_len = pdu_len = MSTP_Create_Frame(
MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */ &mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac, mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length); mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
(void)Ringbuf_Pop(&poSharedData->PDU_Queue, NULL); (void)Ringbuf_Pop(&poSharedData->PDU_Queue, NULL);
@@ -393,14 +397,13 @@ uint16_t MSTP_Get_Send(
* @param nbytes - number of bytes of data to send * @param nbytes - number of bytes of data to send
*/ */
void MSTP_Send_Frame( void MSTP_Send_Frame(
struct mstp_port_struct_t *mstp_port, struct mstp_port_struct_t *mstp_port, uint8_t *buffer, uint16_t nbytes)
uint8_t * buffer,
uint16_t nbytes)
{ {
RS485_Send_Frame(mstp_port, buffer, 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, uint16_t request_pdu_len,
uint8_t src_address, uint8_t src_address,
uint8_t *reply_pdu, uint8_t *reply_pdu,
@@ -427,23 +430,18 @@ bool dlmstp_compare_data_expecting_reply(uint8_t *request_pdu,
/* decode the request data */ /* decode the request data */
request.address.mac[0] = src_address; request.address.mac[0] = src_address;
request.address.mac_len = 1; request.address.mac_len = 1;
offset = bacnet_npdu_decode(request_pdu, request_pdu_len, NULL, offset = bacnet_npdu_decode(
&request.address, &request.npdu_data); request_pdu, request_pdu_len, NULL, &request.address,
&request.npdu_data);
if (request.npdu_data.network_layer_message) { if (request.npdu_data.network_layer_message) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Request is Network message.\n"); "Request is Network message.\n");
#endif
return false; return false;
} }
request.pdu_type = request_pdu[offset] & 0xF0; request.pdu_type = request_pdu[offset] & 0xF0;
if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) { if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Not Confirmed Request.\n"); "Not Confirmed Request.\n");
#endif
return false; return false;
} }
request.invoke_id = request_pdu[offset + 2]; 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( offset = bacnet_npdu_decode(
reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data); reply_pdu, reply_pdu_len, &reply.address, NULL, &reply.npdu_data);
if (reply.npdu_data.network_layer_message) { if (reply.npdu_data.network_layer_message) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Reply is Network message.\n"); "Reply is Network message.\n");
#endif
return false; return false;
} }
/* reply could be a lot of things: /* 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) || if ((reply.pdu_type == PDU_TYPE_REJECT) ||
(reply.pdu_type == PDU_TYPE_ABORT)) { (reply.pdu_type == PDU_TYPE_ABORT)) {
if (request.invoke_id != reply.invoke_id) { if (request.invoke_id != reply.invoke_id) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n"); "Invoke ID mismatch.\n");
#endif
return false; return false;
} }
} else { } else {
if (request.invoke_id != reply.invoke_id) { if (request.invoke_id != reply.invoke_id) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Invoke ID mismatch.\n"); "Invoke ID mismatch.\n");
#endif
return false; return false;
} }
if (request.service_choice != reply.service_choice) { if (request.service_choice != reply.service_choice) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"Service choice mismatch.\n"); "Service choice mismatch.\n");
#endif
return false; return false;
} }
} }
if (request.npdu_data.protocol_version != if (request.npdu_data.protocol_version !=
reply.npdu_data.protocol_version) { reply.npdu_data.protocol_version) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"NPDU Protocol Version mismatch.\n"); "NPDU Protocol Version mismatch.\n");
#endif
return false; return false;
} }
#if 0 #if 0
/* the NDPU priority doesn't get passed through the stack, and /* the NDPU priority doesn't get passed through the stack, and
all outgoing messages have NORMAL priority */ all outgoing messages have NORMAL priority */
if (request.npdu_data.priority != reply.npdu_data.priority) { if (request.npdu_data.priority != reply.npdu_data.priority) {
#if PRINT_ENABLED debug_printf(
fprintf(stderr,
"DLMSTP: DER Compare failed: " "NPDU Priority mismatch.\n"); "DLMSTP: DER Compare failed: " "NPDU Priority mismatch.\n");
#endif
return false; return false;
} }
#endif #endif
if (!bacnet_address_same(&request.address, &reply.address)) { if (!bacnet_address_same(&request.address, &reply.address)) {
#if PRINT_ENABLED debug_printf("DLMSTP: DER Compare failed: "
fprintf(stderr,
"DLMSTP: DER Compare failed: "
"BACnet Address mismatch.\n"); "BACnet Address mismatch.\n");
#endif
return false; 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 */ /* Get the reply to a DATA_EXPECTING_REPLY frame, or nothing */
uint16_t MSTP_Get_Reply( uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
struct mstp_port_struct_t *mstp_port, unsigned timeout)
{ /* milliseconds to wait for a packet */ { /* milliseconds to wait for a packet */
uint16_t pdu_len = 0; /* return value */ uint16_t pdu_len = 0; /* return value */
bool matched = false; bool matched = false;
@@ -574,9 +551,10 @@ uint16_t MSTP_Get_Reply(
} }
pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&poSharedData->PDU_Queue); pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&poSharedData->PDU_Queue);
/* is this the reply to the DER? */ /* is this the reply to the DER? */
matched = dlmstp_compare_data_expecting_reply(&mstp_port->InputBuffer[0], matched = dlmstp_compare_data_expecting_reply(
mstp_port->DataLength, mstp_port->SourceAddress, &mstp_port->InputBuffer[0], mstp_port->DataLength,
(uint8_t *)&pkt->buffer[0], pkt->length, pkt->destination_mac); mstp_port->SourceAddress, (uint8_t *)&pkt->buffer[0], pkt->length,
pkt->destination_mac);
if (!matched) { if (!matched) {
/* Walk the rest of the ring buffer to see if we can find a match */ /* Walk the rest of the ring buffer to see if we can find a match */
while (!matched && while (!matched &&
@@ -598,8 +576,8 @@ uint16_t MSTP_Get_Reply(
frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY; frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
} }
/* convert the PDU into the MSTP Frame */ /* convert the PDU into the MSTP Frame */
pdu_len = pdu_len = MSTP_Create_Frame(
MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */ &mstp_port->OutputBuffer[0], /* <-- loading this */
mstp_port->OutputBufferSize, frame_type, pkt->destination_mac, mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length); mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
/* This will pop the element no matter where we found it */ /* This will pop the element no matter where we found it */
@@ -610,26 +588,13 @@ uint16_t MSTP_Get_Reply(
void dlmstp_set_mac_address(void *poPort, uint8_t mac_address) 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; struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) { if (!mstp_port) {
return; return;
} }
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return;
}
*/
/* Master Nodes can only have address 0-127 */ /* Master Nodes can only have address 0-127 */
if (mac_address <= 127) { if (mac_address <= 127) {
mstp_port->This_Station = mac_address; 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) if (mac_address > mstp_port->Nmax_master)
dlmstp_set_max_master(mstp_port, mac_address); 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) uint8_t dlmstp_mac_address(void *poPort)
{ {
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort; struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) { if (!mstp_port) {
return 0; return 0;
} }
/* poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return 0;
}
*/
return mstp_port->This_Station; return mstp_port->This_Station;
} }
@@ -663,25 +621,13 @@ uint8_t dlmstp_mac_address(void *poPort)
/* node, its value shall be 1. */ /* node, its value shall be 1. */
void dlmstp_set_max_info_frames(void *poPort, uint8_t max_info_frames) 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; struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) { if (!mstp_port) {
return; return;
} }
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return;
}
*/
if (max_info_frames >= 1) { if (max_info_frames >= 1) {
mstp_port->Nmax_info_frames = max_info_frames; 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; 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) 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; struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) { if (!mstp_port) {
return 0; return 0;
} }
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return 0;
}
*/
return mstp_port->Nmax_info_frames; return mstp_port->Nmax_info_frames;
} }
@@ -711,26 +650,13 @@ uint8_t dlmstp_max_info_frames(void *poPort)
/* its value shall be 127. */ /* its value shall be 127. */
void dlmstp_set_max_master(void *poPort, uint8_t max_master) 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; struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) { if (!mstp_port) {
return; return;
} }
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return;
}
*/
if (max_master <= 127) { if (max_master <= 127) {
if (mstp_port->This_Station <= max_master) { if (mstp_port->This_Station <= max_master) {
mstp_port->Nmax_master = 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) uint8_t dlmstp_max_master(void *poPort)
{ {
/* SHARED_MSTP_DATA * poSharedData; */
struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort; struct mstp_port_struct_t *mstp_port = (struct mstp_port_struct_t *)poPort;
if (!mstp_port) { if (!mstp_port) {
return 0; return 0;
} }
/*
poSharedData = (SHARED_MSTP_DATA *) mstp_port->UserData;
if(!poSharedData)
{
return 0;
}
*/
return mstp_port->Nmax_master; return mstp_port->Nmax_master;
} }
@@ -873,14 +793,16 @@ bool dlmstp_init(void *poPort, char *ifname)
poSharedData->RS485_Port_Name = ifname; poSharedData->RS485_Port_Name = ifname;
/* initialize PDU queue */ /* 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); sizeof(struct mstp_pdu_packet), MSTP_PDU_PACKET_COUNT);
/* initialize packet queue */ /* initialize packet queue */
poSharedData->Receive_Packet.ready = false; poSharedData->Receive_Packet.ready = false;
poSharedData->Receive_Packet.pdu_len = 0; poSharedData->Receive_Packet.pdu_len = 0;
rv = sem_init(&poSharedData->Receive_Packet_Flag, 0, 0); rv = sem_init(&poSharedData->Receive_Packet_Flag, 0, 0);
if (rv != 0) { if (rv != 0) {
fprintf(stderr, fprintf(
stderr,
"MS/TP Interface: %s\n cannot allocate PThread Condition.\n", "MS/TP Interface: %s\n cannot allocate PThread Condition.\n",
ifname); ifname);
exit(1); exit(1);
@@ -892,7 +814,8 @@ bool dlmstp_init(void *poPort, char *ifname)
Open device for reading and writing. Open device for reading and writing.
Blocking mode - more CPU effecient 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 */); O_RDWR | O_NOCTTY | O_NONBLOCK /*| O_NDELAY */);
if (poSharedData->RS485_Handle < 0) { if (poSharedData->RS485_Handle < 0) {
perror(poSharedData->RS485_Port_Name); perror(poSharedData->RS485_Port_Name);
@@ -930,7 +853,8 @@ bool dlmstp_init(void *poPort, char *ifname)
usleep(200000); usleep(200000);
tcflush(poSharedData->RS485_Handle, TCIOFLUSH); tcflush(poSharedData->RS485_Handle, TCIOFLUSH);
/* ringbuffer */ /* ringbuffer */
FIFO_Init(&poSharedData->Rx_FIFO, poSharedData->Rx_Buffer, FIFO_Init(
&poSharedData->Rx_FIFO, poSharedData->Rx_Buffer,
sizeof(poSharedData->Rx_Buffer)); sizeof(poSharedData->Rx_Buffer));
printf("=success!\n"); printf("=success!\n");
mstp_port->InputBuffer = &poSharedData->RxBuffer[0]; mstp_port->InputBuffer = &poSharedData->RxBuffer[0];
@@ -941,12 +865,10 @@ bool dlmstp_init(void *poPort, char *ifname)
mstp_port->SilenceTimer = Timer_Silence; mstp_port->SilenceTimer = Timer_Silence;
mstp_port->SilenceTimerReset = Timer_Silence_Reset; mstp_port->SilenceTimerReset = Timer_Silence_Reset;
MSTP_Init(mstp_port); MSTP_Init(mstp_port);
#if PRINT_ENABLED debug_fprintf(stderr, "MS/TP MAC: %02X\n", mstp_port->This_Station);
fprintf(stderr, "MS/TP MAC: %02X\n", mstp_port->This_Station); debug_fprintf(stderr, "MS/TP Max_Master: %02X\n", mstp_port->Nmax_master);
fprintf(stderr, "MS/TP Max_Master: %02X\n", mstp_port->Nmax_master); debug_fprintf(
fprintf(stderr, "MS/TP Max_Info_Frames: %u\n", mstp_port->Nmax_info_frames); stderr, "MS/TP Max_Info_Frames: %u\n", mstp_port->Nmax_info_frames);
#endif
rv = pthread_create(&hThread, NULL, dlmstp_master_fsm_task, mstp_port); rv = pthread_create(&hThread, NULL, dlmstp_master_fsm_task, mstp_port);
if (rv != 0) { if (rv != 0) {
fprintf(stderr, "Failed to start Master Node FSM task\n"); 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); va_end(ap);
fflush(stream); fflush(stream);
#else #else
(void)stream;
(void)format; (void)format;
#endif #endif
return length; return length;