Fix mstp on bsd port (#1193)

* fix mstp on macos NS_PER_S is 1s and 0ns

* fix compile on freebsd

* port bsd: add compile warnings and comments

* port bsd: limit pthred prio to OS defined min/max value
e.g. FreeBSD min=0 max=31

* port bsd: fix clangformat

CMakeLists.txt

@@ -159,6 +159,17 @@ if (CMAKE_C_COMPILER_ID MATCHES "Clang" OR CMAKE_C_COMPILER_ID MATCHES "AppleCla
     add_compile_options(-Wno-long-long)
   endif()
 
+  if(${CMAKE_SYSTEM_NAME} STREQUAL "FreeBSD")
+    # These warnings orginate from libdispatch
+    # ISO C restricts enumerator values to range of 'int'
+    # DISPATCH_ENUM_API_AVAILABLE(macos(10.14), ios(12.0), tvos(12.0), watchos(5.0))    = ~1ull
+    add_compile_options(-Wno-pedantic)
+    # These warnings originate from libdispatch headers,
+    # which rely on Clang extensions that Apple Clang supports,
+    # but FreeBSD LLVM Clang does not.
+    add_compile_options(-Wno-nullability-extension)
+  endif()
+
   # Should be fixed in a future.
   add_compile_options(-Wno-cast-qual)
   add_compile_options(-Wno-double-promotion)
@@ -839,6 +850,7 @@ elseif(APPLE)
   message(STATUS "BACNET: building for APPLE")
   set(BACNET_PORT_DIRECTORY_PATH ${CMAKE_CURRENT_LIST_DIR}/ports/bsd)
   add_compile_definitions(BACNET_PORT=bsd)
+  add_compile_definitions(USE_MACH_TIME)
   include_directories(ports/posix)
 
   target_sources(${PROJECT_NAME} PRIVATE
@@ -884,6 +896,18 @@ elseif(${CMAKE_SYSTEM_NAME} STREQUAL "FreeBSD")
   message(STATUS "BACNET: building for FreeBSD")
   set(BACNET_PORT_DIRECTORY_PATH ${CMAKE_CURRENT_LIST_DIR}/ports/bsd)
   include_directories(ports/posix)
+  link_directories(/usr/lib)
+  link_libraries(c)
+  link_libraries(m)
+  FIND_PATH(DISPATCH_INCLUDE dispatch/dispatch.h)
+  FIND_LIBRARY(DISPATCH_LIBRARIES NAMES dispatch)
+  if(NOT DISPATCH_INCLUDE OR NOT DISPATCH_LIBRARIES)
+      message(FATAL_ERROR "BACNET: dispatch.h header file not found
+      install libdispatch with 'pkg install libdispatch'")
+  else()
+      include_directories(${DISPATCH_INCLUDE})
+      link_libraries(${DISPATCH_LIBRARIES})
+  endif()
 
   target_sources(${PROJECT_NAME} PRIVATE
     ports/bsd/bacport.h
@@ -1091,6 +1115,7 @@ if(BACNET_STACK_BUILD_APPS)
     if(NOT LIBCONFIG_LIBRARIES)
       message(WARNING "BACNET: Will not build apps/router as libconfig not found")
     else()
+      message(STATUS "BACNET: Will build apps/router with libconfig")
       add_executable(
         router
         ${BACNET_PORT_DIRECTORY_PATH}/dlmstp_port.c

ports/bsd/dlmstp.c

@@ -29,7 +29,11 @@
 #include "rs485.h"
 
 /* packet queues */
-static DLMSTP_PACKET Receive_Packet;
+#ifndef MSTP_RECEIVE_PACKET_COUNT
+#define MSTP_RECEIVE_PACKET_COUNT 8
+#endif
+static DLMSTP_PACKET Receive_Buffer[MSTP_RECEIVE_PACKET_COUNT];
+static RING_BUFFER Receive_Queue;
 /* mechanism to wait for a packet */
 static pthread_cond_t Receive_Packet_Flag;
 static pthread_mutex_t Receive_Packet_Mutex;
@@ -127,6 +131,9 @@ int dlmstp_send_pdu(
         }
     }
     pthread_mutex_unlock(&Ring_Buffer_Mutex);
+    if (!pkt) {
+        debug_printf("DLMSTP: PDU Queue Full!\n");
+    }
 
     return bytes_sent;
 }
@@ -166,6 +173,89 @@ uint16_t MSTP_Get_Send(struct mstp_port_struct_t *mstp_port, unsigned timeout)
     return pdu_len;
 }
 
+#define TIMING_GIGA (1000000000)
+
+/* timespec difference (monotonic) right - left */
+void timespec_monodiff_rml(
+    struct timespec *ts_out, const struct timespec *ts_in)
+{
+    /* out = in - out,
+       where in > out
+     */
+    ts_out->tv_sec = ts_in->tv_sec - ts_out->tv_sec;
+    ts_out->tv_nsec = ts_in->tv_nsec - ts_out->tv_nsec;
+    if (ts_out->tv_sec < 0) {
+        ts_out->tv_sec = 0;
+        ts_out->tv_nsec = 0;
+    } else if (ts_out->tv_nsec < 0) {
+        if (ts_out->tv_sec == 0) {
+            ts_out->tv_sec = 0;
+            ts_out->tv_nsec = 0;
+        } else {
+            ts_out->tv_sec = ts_out->tv_sec - 1;
+            ts_out->tv_nsec = ts_out->tv_nsec + TIMING_GIGA;
+        }
+    }
+}
+
+/* emulate clock_nanosleep for CLOCK_MONOTONIC and TIMER_ABSTIME */
+int clock_nanosleep_abstime(const struct timespec *req)
+{
+    struct timespec ts_delta;
+    int retval = clock_gettime(CLOCK_MONOTONIC, &ts_delta);
+    if (retval == 0) {
+        timespec_monodiff_rml(&ts_delta, req);
+        retval = nanosleep(&ts_delta, NULL);
+    }
+    return retval;
+}
+
+/**
+ * Add a certain number of nanoseconds to the specified time.
+ *
+ * @param ts - The time to which to add to.
+ * @param ns - The number of nanoseconds to add.  Allowed range
+ *      is -NS_PER_S..NS_PER_S (i.e., plus minus one second).
+ */
+static void timespec_add_ns(struct timespec *ts, long ns)
+{
+    /* nano-seconds per second */
+    const long NS_PER_S = 1000000000L;
+
+    ts->tv_nsec += ns;
+    if (ts->tv_nsec >= NS_PER_S) {
+        ts->tv_nsec -= NS_PER_S;
+        ts->tv_sec += 1;
+    } else if (ts->tv_nsec < 0) {
+        ts->tv_nsec += NS_PER_S;
+        ts->tv_sec -= 1;
+    }
+}
+
+/**
+ * @brief Get abstime for use in thread
+ * @param abstime - place to put the absolute time
+ * @param milliseconds - number of milliseconds to add
+ */
+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",
+            milliseconds);
+        milliseconds = 1000;
+    }
+    timespec_add_ns(abstime, 1000000 * milliseconds);
+}
+
+static void millisleep(const unsigned long milliseconds)
+{
+    struct timespec abstime;
+    get_abstime(&abstime, milliseconds);
+    while (EINTR == clock_nanosleep_abstime(&abstime)) { }
+}
+
 /**
  * @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
@@ -179,20 +269,22 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
     bool matched = false;
     uint8_t frame_type = 0;
     struct mstp_pdu_packet *pkt;
-
     (void)timeout;
-    if (Ringbuf_Empty(&PDU_Queue)) {
+
-        return 0;
+    pthread_mutex_lock(&Ring_Buffer_Mutex);
-    }
+    for (pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue); pkt;
-    pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
+         pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(
+             &PDU_Queue, (uint8_t *)pkt)) {
         /* is this the reply to the DER? */
         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);
-    if (!matched) {
+        if (matched) {
-        return 0;
+            break;
         }
+    }
+    if (matched) {
         if (pkt->data_expecting_reply) {
             frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
         } else {
@@ -204,7 +296,14 @@ uint16_t MSTP_Get_Reply(struct mstp_port_struct_t *mstp_port, unsigned timeout)
             mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
             mstp_port->This_Station, (uint8_t *)&pkt->buffer[0], pkt->length);
         DLMSTP_Statistics.transmit_pdu_counter++;
-    (void)Ringbuf_Pop(&PDU_Queue, NULL);
+        /* This will pop the element no matter where we found it */
+        (void)Ringbuf_Pop_Element(&PDU_Queue, (uint8_t *)pkt, NULL);
+    }
+    pthread_mutex_unlock(&Ring_Buffer_Mutex);
+    if (pdu_len <= 0) {
+        /* Didn't find a match so wait for application layer to provide one */
+        millisleep(1);
+    }
 
     return pdu_len;
 }
@@ -231,79 +330,40 @@ void MSTP_Send_Frame(
 uint16_t MSTP_Put_Receive(struct mstp_port_struct_t *mstp_port)
 {
     uint16_t pdu_len = 0;
+    DLMSTP_PACKET *pkt;
 
     pthread_mutex_lock(&Receive_Packet_Mutex);
-    if (Receive_Packet.ready) {
+    pkt = (DLMSTP_PACKET *)Ringbuf_Data_Peek(&Receive_Queue);
+    if (!pkt) {
         debug_printf("MS/TP: Dropped! Not Ready.\n");
     } else {
         /* bounds check - maybe this should send an abort? */
         pdu_len = mstp_port->DataLength;
-        if (pdu_len > sizeof(Receive_Packet.pdu)) {
+        if (pdu_len > sizeof(pkt->pdu)) {
-            pdu_len = sizeof(Receive_Packet.pdu);
+            pdu_len = sizeof(pkt->pdu);
         }
         if (pdu_len == 0) {
             debug_printf("MS/TP: PDU Length is 0!\n");
         }
         memmove(
-            (void *)&Receive_Packet.pdu[0], (void *)&mstp_port->InputBuffer[0],
+            (void *)&pkt->pdu[0], (void *)&mstp_port->InputBuffer[0], pdu_len);
-            pdu_len);
+        dlmstp_fill_bacnet_address(&pkt->address, mstp_port->SourceAddress);
-        dlmstp_fill_bacnet_address(
+        pkt->pdu_len = mstp_port->DataLength;
-            &Receive_Packet.address, mstp_port->SourceAddress);
+        pkt->ready = true;
-        Receive_Packet.pdu_len = mstp_port->DataLength;
+        if (Ringbuf_Data_Put(&Receive_Queue, (uint8_t *)pkt)) {
-        Receive_Packet.ready = true;
             pthread_cond_signal(&Receive_Packet_Flag);
         }
+    }
     pthread_mutex_unlock(&Receive_Packet_Mutex);
 
     return pdu_len;
 }
 
-/**
- * Add a certain number of nanoseconds to the specified time.
- *
- * @param ts - The time to which to add to.
- * @param ns - The number of nanoseconds to add.  Allowed range
- *      is -NS_PER_S..NS_PER_S (i.e., plus minus one second).
- */
-static void timespec_add_ns(struct timespec *ts, long ns)
-{
-    /* nano-seconds per second */
-    const long NS_PER_S = 1000000000L;
-
-    ts->tv_nsec += ns;
-    if (ts->tv_nsec > NS_PER_S) {
-        ts->tv_nsec -= NS_PER_S;
-        ts->tv_sec += 1;
-    } else if (ts->tv_nsec < 0) {
-        ts->tv_nsec += NS_PER_S;
-        ts->tv_sec -= 1;
-    }
-}
-
-/**
- * @brief Get abstime for use in thread
- * @param abstime - place to put the absolute time
- * @param milliseconds - number of milliseconds to add
- */
-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",
-            milliseconds);
-        milliseconds = 1000;
-    }
-    timespec_add_ns(abstime, 1000000 * milliseconds);
-}
-
 /**
  * @brief Run the MS/TP state machines, and get packet if available
  * @param pdu - place to put PDU data for the caller
  * @param max_pdu - number of bytes of PDU data that caller can receive
  * @return number of bytes in received packet, or 0 if no packet was received
- * @note Must be called at least once every 1 milliseconds, with no more than
- *  5 milliseconds jitter.
  */
 uint16_t dlmstp_receive(
     BACNET_ADDRESS *src, /* source address */
@@ -313,28 +373,32 @@ uint16_t dlmstp_receive(
 { /* milliseconds to wait for a packet */
     uint16_t pdu_len = 0;
     struct timespec abstime;
-
+    DLMSTP_PACKET *pkt;
     (void)max_pdu;
-    /* see if there is a packet available, and a place
+
-       to put the reply (if necessary) and process it */
     pthread_mutex_lock(&Receive_Packet_Mutex);
+    if (timeout > 0) {
         get_abstime(&abstime, timeout);
         pthread_cond_timedwait(
             &Receive_Packet_Flag, &Receive_Packet_Mutex, &abstime);
-    if (Receive_Packet.ready) {
+    }
-        if (Receive_Packet.pdu_len) {
+
+    /* see if there is a packet available, and a place
+       to put the reply (if necessary) and process it */
+    pkt = (DLMSTP_PACKET *)Ringbuf_Peek(&Receive_Queue);
+    if (pkt) {
+        if (pkt->pdu_len) {
             DLMSTP_Statistics.receive_pdu_counter++;
             if (src) {
-                memmove(
+                memmove(src, &pkt->address, sizeof(pkt->address));
-                    src, &Receive_Packet.address,
-                    sizeof(Receive_Packet.address));
             }
             if (pdu) {
-                memmove(pdu, &Receive_Packet.pdu, sizeof(Receive_Packet.pdu));
+                memmove(pdu, &pkt->pdu, sizeof(pkt->pdu));
             }
-            pdu_len = Receive_Packet.pdu_len;
+            pdu_len = pkt->pdu_len;
         }
-        Receive_Packet.ready = false;
+        pkt->ready = false;
+        (void)Ringbuf_Pop(&Receive_Queue, NULL);
     }
     pthread_mutex_unlock(&Receive_Packet_Mutex);
 
@@ -388,14 +452,11 @@ static void *dlmstp_thread(void *pArg)
                     MSTP_Port.DataLength);
             }
             run_master = true;
+            /* we don't run the master state machine for this frame */
+            MSTP_Port.ReceivedValidFrameNotForUs = false;
         } else if (MSTP_Port.ReceivedInvalidFrame) {
-            DLMSTP_Statistics.receive_invalid_frame_counter++;
-            if (MSTP_Port.HeaderCRC != 0x55) {
-                DLMSTP_Statistics.bad_crc_counter++;
-            } else if (MSTP_Port.DataCRC != 0xF0B8) {
-                DLMSTP_Statistics.bad_crc_counter++;
-            }
             if (Invalid_Frame_Rx_Callback) {
+                DLMSTP_Statistics.receive_invalid_frame_counter++;
                 Invalid_Frame_Rx_Callback(
                     MSTP_Port.SourceAddress, MSTP_Port.DestinationAddress,
                     MSTP_Port.FrameType, MSTP_Port.InputBuffer,
@@ -769,16 +830,6 @@ void dlmstp_set_invalid_frame_rx_complete_callback(
     Invalid_Frame_Rx_Callback = cb_func;
 }
 
-/**
- * @brief Set the MS/TP Frame Complete callback
- * @param cb_func - callback function to be called when a frame is received
- */
-void dlmstp_set_invalid_frame_rx_complete_callback(
-    dlmstp_hook_frame_rx_complete_cb cb_func)
-{
-    Invalid_Frame_Rx_Callback = cb_func;
-}
-
 /**
  * @brief Set the MS/TP Preamble callback
  * @param cb_func - callback function to be called when a preamble is received
@@ -897,6 +948,8 @@ const char *dlmstp_get_interface(void)
  */
 bool dlmstp_init(char *ifname)
 {
+    pthread_attr_t thread_attr;
+    struct sched_param sch_param;
     pthread_condattr_t attr;
     int rv = 0;
 
@@ -913,12 +966,14 @@ bool dlmstp_init(char *ifname)
     }
     pthread_condattr_init(&attr);
     // TODO use mach_absolute_time() <mach/mach_time.h> for MONOTONIC clock
+#ifndef USE_MACH_TIME
     if ((rv = pthread_condattr_setclock(&attr, CLOCK_MONOTONIC)) != 0) {
         fprintf(
             stderr, "MS/TP Interface: %s\n failed to set MONOTONIC clock\n",
             ifname);
         exit(1);
     }
+#endif
     pthread_mutex_init(&Thread_Mutex, NULL);
     rv = pthread_mutex_init(&Ring_Buffer_Mutex, NULL);
     if (rv != 0) {
@@ -932,8 +987,9 @@ bool dlmstp_init(char *ifname)
         &PDU_Queue, (uint8_t *)&PDU_Buffer, sizeof(struct mstp_pdu_packet),
         MSTP_PDU_PACKET_COUNT);
     /* initialize packet queue */
-    Receive_Packet.ready = false;
+    Ringbuf_Init(
-    Receive_Packet.pdu_len = 0;
+        &Receive_Queue, (uint8_t *)&Receive_Buffer, sizeof(DLMSTP_PACKET),
+        MSTP_RECEIVE_PACKET_COUNT);
     rv = pthread_cond_init(&Receive_Packet_Flag, &attr);
     if (rv != 0) {
         fprintf(
@@ -965,37 +1021,91 @@ bool dlmstp_init(char *ifname)
     MSTP_Port.BaudRateSet = dlmstp_set_baud_rate;
     MSTP_Init(&MSTP_Port);
 #if PRINT_ENABLED
-    fprintf(stderr, "MS/TP MAC: %02X\n", MSTP_Port.This_Station);
+    debug_fprintf(stderr, "MS/TP MAC: %02X\n", MSTP_Port.This_Station);
-    fprintf(stderr, "MS/TP Max_Master: %02X\n", MSTP_Port.Nmax_master);
+    debug_fprintf(stderr, "MS/TP Max_Master: %02X\n", MSTP_Port.Nmax_master);
-    fprintf(
+    debug_fprintf(
         stderr, "MS/TP Max_Info_Frames: %u\n",
         (unsigned)MSTP_Port.Nmax_info_frames);
-    fprintf(
+    debug_fprintf(
         stderr, "MS/TP RxBuf[%u] TxBuf[%u]\n",
         (unsigned)MSTP_Port.InputBufferSize,
         (unsigned)MSTP_Port.OutputBufferSize);
-    fprintf(
+    debug_fprintf(
         stderr,
         "MS/TP SlaveModeEnabled"
         ": %s\n",
         (MSTP_Port.SlaveNodeEnabled ? "true" : "false"));
-    fprintf(
+    debug_fprintf(
         stderr,
         "MS/TP ZeroConfigEnabled"
         ": %s\n",
         (MSTP_Port.ZeroConfigEnabled ? "true" : "false"));
-    fprintf(
+    debug_fprintf(
         stderr,
         "MS/TP CheckAutoBaud"
         ": %s\n",
         (MSTP_Port.CheckAutoBaud ? "true" : "false"));
     fflush(stderr);
 #endif
+    pthread_attr_init(&thread_attr);
+
+    /* Set scheduling policy to SCHED_FIFO and priority */
+    rv = pthread_attr_setinheritsched(&thread_attr, PTHREAD_EXPLICIT_SCHED);
+    if (rv != 0) {
+        fprintf(
+            stderr,
+            "MS/TP Interface: %s\n cannot setup thread schedule to "
+            "explicit.\n",
+            ifname);
+        exit(1);
+    }
+    rv = pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+    if (rv != 0) {
+        fprintf(
+            stderr,
+            "MS/TP Interface: %s\n cannot setup thread schedule policy to "
+            "FIFO.\n",
+            ifname);
+        exit(1);
+    }
+    sch_param.sched_priority = 99;
+    if (sched_get_priority_max(SCHED_FIFO) < sch_param.sched_priority) {
+        sch_param.sched_priority = sched_get_priority_max(SCHED_FIFO);
+        fprintf(
+            stderr, "MS/TP Interface: setup thread max priority %i\n",
+            sch_param.sched_priority);
+    } else if (sched_get_priority_min(SCHED_FIFO) > sch_param.sched_priority) {
+        sch_param.sched_priority = sched_get_priority_min(SCHED_FIFO);
+        fprintf(
+            stderr, "MS/TP Interface: setup thread min priority %i\n",
+            sch_param.sched_priority);
+    }
+    rv = pthread_attr_setschedparam(&thread_attr, &sch_param);
+    if (rv != 0) {
+        fprintf(
+            stderr, "MS/TP Interface: %s\n cannot setup thread priority.\n",
+            ifname);
+        exit(1);
+    }
     /* start one thread */
     Thread_Run = true;
+    rv = pthread_create(&hThread, &thread_attr, dlmstp_thread, NULL);
+    if (rv == EPERM) {
+        fprintf(
+            stdout,
+            "MS/TP Interface: %s\n"
+            "  Insufficient permissions to create thread with priority.\n"
+            "    A thread without priority will be created.\n"
+            "  Run this executable as a user with thread priority permission\n"
+            "    or grant capability with \"setcap 'cap_sys_nice=eip'\"",
+            ifname);
         rv = pthread_create(&hThread, NULL, dlmstp_thread, NULL);
+    }
     if (rv != 0) {
-        fprintf(stderr, "Failed to start MS/TP thread\n");
+        fprintf(
+            stderr, "MS/TP Interface: %s\n Failed to start MS/TP thread.\n",
+            ifname);
+        exit(1);
     }
 
     return true;
@@ -1018,9 +1128,9 @@ static char *Network_Interface = NULL;
 
 int main(int argc, char *argv[])
 {
-    uint16_t pdu_len = 0;
+    uint16_t pdu_len;
 
-    /* argv has the "COM4" or some other device */
+    /* argv has the "/dev/ttyUSB0" or some other device */
     if (argc > 1) {
         Network_Interface = argv[1];
     }
@@ -1035,6 +1145,7 @@ int main(int argc, char *argv[])
         MSTP_Create_And_Send_Frame(
             &MSTP_Port, FRAME_TYPE_TEST_REQUEST, MSTP_Port.SourceAddress,
             MSTP_Port.This_Station, NULL, 0);
+        (void)pdu_len;
     }
 
     return 0;

ports/bsd/dlmstp_port.c

@@ -11,14 +11,16 @@
 #include <stdlib.h>
 #include <string.h>
 #include <stdio.h>
-#include <sys/time.h>
+#include <time.h>
-#include <errno.h>
 /* BSD includes */
+#if defined(__APPLE__) || defined(__darwin__)
 #include <IOKit/serial/ioss.h>
+#else
+#include <sys/serial.h>
+#endif
 /* BACnet Stack defines - first */
 #include "bacnet/bacdef.h"
 /* BACnet Stack API */
-#include "bacnet/bacdef.h"
 #include "bacnet/bacaddr.h"
 #include "bacnet/npdu.h"
 #include "bacnet/datalink/mstp.h"

ports/bsd/readme.txt

@@ -1 +1,10 @@
 This is a port to MAC OS X or FreeBSD for testing.
+
+Install on FreeBSD 15
+sudo pkg install cmake git libdispatch
+git clone https://github.com/bacnet-stack/bacnet-stack.git
+cd bacnet-stack
+mkdir build
+cd build
+cmake ..
+make

ports/bsd/rs485.c

@@ -36,6 +36,7 @@
 #include "bacnet/datalink/mstp.h"
 #include "rs485.h"
 #include "bacnet/basic/sys/fifo.h"
+#include "bacnet/basic/sys/debug.h"
 
 #include <sys/select.h>
 #include <sys/time.h>
@@ -398,72 +399,45 @@ bool RS485_Set_Baud_Rate(uint32_t baud)
 void RS485_Send_Frame(
     struct mstp_port_struct_t *mstp_port, /* port specific data */
     const uint8_t *buffer, /* frame to send (up to 501 bytes of data) */
-    uint16_t nbytes)
+    uint16_t nbytes /* number of bytes of data (up to 501) */)
-{ /* number of bytes of data (up to 501) */
+{
     uint32_t turnaround_time_usec = Tturnaround * 1000000UL;
-    uint32_t baud;
+    uint32_t baud = RS485_Baud;
+    int handle = RS485_Handle;
     ssize_t written = 0;
     int greska;
-    SHARED_MSTP_DATA *poSharedData = NULL;
+    const SHARED_MSTP_DATA *poSharedData = NULL;
 
-    if (mstp_port) {
+    if (mstp_port && mstp_port->UserData) {
         poSharedData = (SHARED_MSTP_DATA *)mstp_port->UserData;
-    }
+        baud = poSharedData->RS485_Baud;
-    if (!poSharedData) {
+        handle = poSharedData->RS485_Handle;
-        baud = RS485_Get_Baud_Rate();
-        /* sleeping for turnaround time is necessary to give other devices
-           time to change from sending to receiving state. */
-        usleep(turnaround_time_usec / baud);
-        /*
-           On  success,  the  number of bytes written are returned (zero
-           indicates nothing was written).  On error, -1  is  returned,  and
-           errno  is  set appropriately.   If  count  is zero and the file
-           descriptor refers to a regular file, 0 will be returned without
-           causing any other effect.  For a special file, the results are not
-           portable.
-         */
-        written = write(RS485_Handle, buffer, nbytes);
-        greska = errno;
-        if (written <= 0) {
-            printf("write error: %s\n", strerror(greska));
-        } else {
-            /* wait until all output has been transmitted. */
-            tcdrain(RS485_Handle);
-        }
-        /*  tcdrain(RS485_Handle); */
-        /* per MSTP spec, sort of */
-        if (mstp_port) {
-            mstp_port->SilenceTimerReset((void *)mstp_port);
-        }
-    } else {
-        baud = RS485_Get_Port_Baud_Rate(mstp_port);
-        /* sleeping for turnaround time is necessary to give other devices
-           time to change from sending to receiving state. */
-        usleep(turnaround_time_usec / baud);
-        /*
-           On  success,  the  number of bytes written are returned (zero
-           indicates nothing was written).  On error, -1  is  returned,  and
-           errno  is  set appropriately.   If  count  is zero and the file
-           descriptor refers to a regular file, 0 will be returned without
-           causing any other effect.  For a special file, the results are not
-           portable.
-         */
-        written = write(poSharedData->RS485_Handle, buffer, nbytes);
-        greska = errno;
-        if (written <= 0) {
-            printf("write error: %s\n", strerror(greska));
-        } else {
-            /* wait until all output has been transmitted. */
-            tcdrain(poSharedData->RS485_Handle);
-        }
-        /*  tcdrain(RS485_Handle); */
-        /* per MSTP spec, sort of */
-        if (mstp_port) {
-            mstp_port->SilenceTimerReset((void *)mstp_port);
-        }
     }
 
-    return;
+    /* sleeping for turnaround time is necessary to give other devices
+       time to change from sending to receiving state. */
+    usleep(turnaround_time_usec / baud);
+    /*
+       On  success,  the  number of bytes written are returned (zero
+       indicates nothing was written).  On error, -1  is  returned,  and
+       errno  is  set appropriately.   If  count  is zero and the file
+       descriptor refers to a regular file, 0 will be returned without
+       causing any other effect.  For a special file, the results are not
+       portable.
+     */
+    written = write(handle, buffer, nbytes);
+    greska = errno;
+    if (written <= 0) {
+        printf("write error: %s\n", strerror(greska));
+    } else {
+        /* wait until all output has been transmitted. */
+        tcdrain(handle);
+    }
+    /*  tcdrain(RS485_Handle); */
+    /* per MSTP spec, sort of */
+    if (mstp_port) {
+        mstp_port->SilenceTimerReset((void *)mstp_port);
+    }
 }
 
 /****************************************************************************
@@ -477,72 +451,42 @@ void RS485_Check_UART_Data(struct mstp_port_struct_t *mstp_port)
     fd_set input;
     struct timeval waiter;
     uint8_t buf[2048];
-    int n;
+    ssize_t n;
+    int handle = RS485_Handle;
+    SHARED_MSTP_DATA *poSharedData;
+    FIFO_BUFFER *fifo = &Rx_FIFO;
 
-    SHARED_MSTP_DATA *poSharedData = (SHARED_MSTP_DATA *)mstp_port->UserData;
-    if (!poSharedData) {
-        if (mstp_port->ReceiveError == true) {
-            /* do nothing but wait for state machine to clear the error */
-            /* burning time, so wait a longer time */
     waiter.tv_sec = 0;
     waiter.tv_usec = 5000;
+    poSharedData = (SHARED_MSTP_DATA *)mstp_port->UserData;
+    if (poSharedData) {
+        handle = poSharedData->RS485_Handle;
+        fifo = &poSharedData->Rx_FIFO;
+    }
+    if (mstp_port->ReceiveError == true) {
+        /* do nothing but wait for state machine to clear the error */
     } else if (mstp_port->DataAvailable == false) {
         /* wait for state machine to read from the DataRegister */
-            if (FIFO_Count(&Rx_FIFO) > 0) {
+        if (FIFO_Count(fifo) > 0) {
             /* data is available */
-                mstp_port->DataRegister = FIFO_Get(&Rx_FIFO);
+            mstp_port->DataRegister = FIFO_Get(fifo);
             mstp_port->DataAvailable = true;
             /* FIFO is giving data - just poll */
             waiter.tv_sec = 0;
             waiter.tv_usec = 0;
-            } else {
-                /* FIFO is empty - wait a longer time */
-                waiter.tv_sec = 0;
-                waiter.tv_usec = 5000;
         }
     }
     /* grab bytes and stuff them into the FIFO every time */
     FD_ZERO(&input);
-        FD_SET(RS485_Handle, &input);
+    FD_SET(handle, &input);
-        n = select(RS485_Handle + 1, &input, NULL, NULL, &waiter);
+    n = select(handle + 1, &input, NULL, NULL, &waiter);
     if (n < 0) {
         return;
     }
-        if (FD_ISSET(RS485_Handle, &input)) {
+    if (FD_ISSET(handle, &input)) {
-            n = read(RS485_Handle, buf, sizeof(buf));
+        n = read(handle, buf, sizeof(buf));
-            FIFO_Add(&Rx_FIFO, &buf[0], n);
+        if (n > 0) {
-        }
+            FIFO_Add(fifo, &buf[0], n);
-    } else {
-        if (mstp_port->ReceiveError == true) {
-            /* do nothing but wait for state machine to clear the error */
-            /* burning time, so wait a longer time */
-            waiter.tv_sec = 0;
-            waiter.tv_usec = 5000;
-        } else if (mstp_port->DataAvailable == false) {
-            /* wait for state machine to read from the DataRegister */
-            if (FIFO_Count(&poSharedData->Rx_FIFO) > 0) {
-                /* data is available */
-                mstp_port->DataRegister = FIFO_Get(&poSharedData->Rx_FIFO);
-                mstp_port->DataAvailable = true;
-                /* FIFO is giving data - just poll */
-                waiter.tv_sec = 0;
-                waiter.tv_usec = 0;
-            } else {
-                /* FIFO is empty - wait a longer time */
-                waiter.tv_sec = 0;
-                waiter.tv_usec = 5000;
-            }
-        }
-        /* grab bytes and stuff them into the FIFO every time */
-        FD_ZERO(&input);
-        FD_SET(poSharedData->RS485_Handle, &input);
-        n = select(poSharedData->RS485_Handle + 1, &input, NULL, NULL, &waiter);
-        if (n < 0) {
-            return;
-        }
-        if (FD_ISSET(poSharedData->RS485_Handle, &input)) {
-            n = read(poSharedData->RS485_Handle, buf, sizeof(buf));
-            FIFO_Add(&poSharedData->Rx_FIFO, &buf[0], n);
         }
     }
 }