diff -ru libmodbus-xeno-3.1.6-src/src/Makefile.am libmodbus-xeno-3.1.6/src/Makefile.am
--- libmodbus-xeno-3.1.6-src/src/Makefile.am	2019-08-01 05:38:15.000000000 +0800
+++ libmodbus-xeno-3.1.6/src/Makefile.am	2022-02-21 15:51:34.482336251 +0800
@@ -7,7 +7,7 @@
     -DLIBEXECDIR=\""$(libexecdir)"\" \
     -I${top_srcdir}/src
 
-AM_CFLAGS = ${my_CFLAGS}
+AM_CFLAGS = ${my_CFLAGS} -I/usr/xenomai/include/ -I/usr/xenomai/include/cobalt
 
 libmodbus_la_SOURCES = \
         modbus.c \
@@ -23,7 +23,7 @@
         modbus-version.h
 
 libmodbus_la_LDFLAGS = -no-undefined \
-        -version-info $(LIBMODBUS_LT_VERSION_INFO)
+        -version-info $(LIBMODBUS_LT_VERSION_INFO) -Wl,--no-as-needed -Wl,@/usr/xenomai/lib/cobalt.wrappers -Wl,@/usr/xenomai/lib/modechk.wrappers  /usr/xenomai/lib/xenomai/bootstrap.lo  -L/usr/xenomai/lib -lcobalt -lmodechk -lpthread -lrt
 
 if OS_WIN32
 libmodbus_la_LIBADD = -lwsock32
diff -ru libmodbus-xeno-3.1.6-src/src/Makefile.in libmodbus-xeno-3.1.6/src/Makefile.in
--- libmodbus-xeno-3.1.6-src/src/Makefile.in	2019-08-01 05:38:45.000000000 +0800
+++ libmodbus-xeno-3.1.6/src/Makefile.in	2022-02-21 16:00:54.454333592 +0800
@@ -349,7 +349,7 @@
     -DLIBEXECDIR=\""$(libexecdir)"\" \
     -I${top_srcdir}/src
 
-AM_CFLAGS = ${my_CFLAGS}
+AM_CFLAGS = ${my_CFLAGS} -I/usr/xenomai/include/ -I/usr/xenomai/include/cobalt
 libmodbus_la_SOURCES = \
         modbus.c \
         modbus.h \
@@ -364,7 +364,7 @@
         modbus-version.h
 
 libmodbus_la_LDFLAGS = -no-undefined \
-        -version-info $(LIBMODBUS_LT_VERSION_INFO)
+        -version-info $(LIBMODBUS_LT_VERSION_INFO) -Wl,--no-as-needed -Wl,@/usr/xenomai/lib/cobalt.wrappers -Wl,@/usr/xenomai/lib/modechk.wrappers  /usr/xenomai/lib/xenomai/bootstrap.lo  -L/usr/xenomai/lib -lcobalt -lmodechk -lpthread -lrt
 
 @OS_QNX_TRUE@libmodbus_la_LIBADD = -lsocket
 @OS_WIN32_TRUE@libmodbus_la_LIBADD = -lwsock32
diff -ru libmodbus-xeno-3.1.6-src/src/modbus.c libmodbus-xeno-3.1.6/src/modbus.c
--- libmodbus-xeno-3.1.6-src/src/modbus.c	2019-08-01 05:38:15.000000000 +0800
+++ libmodbus-xeno-3.1.6/src/modbus.c	2022-02-21 16:13:55.638800448 +0800
@@ -383,25 +383,26 @@
         p_tv = &tv;
     }
 
-    while (length_to_read != 0) {
-        rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
-        if (rc == -1) {
-            _error_print(ctx, "select");
-            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
-                int saved_errno = errno;
+    // only set timeout once
+    rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
+    if (rc == -1) {
+        _error_print(ctx, "select");
+        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
+            int saved_errno = errno;
 
-                if (errno == ETIMEDOUT) {
-                    _sleep_response_timeout(ctx);
-                    modbus_flush(ctx);
-                } else if (errno == EBADF) {
-                    modbus_close(ctx);
-                    modbus_connect(ctx);
-                }
-                errno = saved_errno;
+            if (errno == ETIMEDOUT) {
+                _sleep_response_timeout(ctx);
+                modbus_flush(ctx);
+            } else if (errno == EBADF) {
+                modbus_close(ctx);
+                modbus_connect(ctx);
             }
-            return -1;
+            errno = saved_errno;
         }
+        return -1;
+    }
 
+    while (length_to_read != 0) {
         rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
         if (rc == 0) {
             errno = ECONNRESET;
@@ -528,7 +529,7 @@
     }
 
     rsp_length_computed = compute_response_length_from_request(ctx, req);
-
+    
     /* Exception code */
     if (function >= 0x80) {
         if (rsp_length == (offset + 2 + (int)ctx->backend->checksum_length) &&
diff -ru libmodbus-xeno-3.1.6-src/src/modbus-rtu.c libmodbus-xeno-3.1.6/src/modbus-rtu.c
--- libmodbus-xeno-3.1.6-src/src/modbus-rtu.c	2019-08-01 05:38:15.000000000 +0800
+++ libmodbus-xeno-3.1.6/src/modbus-rtu.c	2022-02-21 16:13:51.198793048 +0800
@@ -18,10 +18,14 @@
 
 #include "modbus-rtu.h"
 #include "modbus-rtu-private.h"
-
+#if 1
+#include <rtdm/rtdm.h>
+#include <rtdm/serial.h>
+#else
 #if HAVE_DECL_TIOCSRS485 || HAVE_DECL_TIOCM_RTS
 #include <sys/ioctl.h>
 #endif
+#endif
 
 #if HAVE_DECL_TIOCSRS485
 #include <linux/serial.h>
@@ -399,8 +403,8 @@
 #if defined(_WIN32)
     DCB dcb;
 #else
-    struct termios tios;
-    speed_t speed;
+    // struct termios tios;
+    // speed_t speed;
     int flags;
 #endif
     modbus_rtu_t *ctx_rtu = ctx->backend_data;
@@ -583,11 +587,11 @@
 
        Timeouts are ignored in canonical input mode or when the
        NDELAY option is set on the file via open or fcntl */
-    flags = O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL;
-#ifdef O_CLOEXEC
-    flags |= O_CLOEXEC;
-#endif
-
+//     flags = O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL;
+// #ifdef O_CLOEXEC
+//     flags |= O_CLOEXEC;
+// #endif
+    flags = O_RDWR;
     ctx->s = open(ctx_rtu->device, flags);
     if (ctx->s == -1) {
         if (ctx->debug) {
@@ -596,7 +600,7 @@
         }
         return -1;
     }
-
+#if 0
     /* Save */
     tcgetattr(ctx->s, &ctx_rtu->old_tios);
 
@@ -893,6 +897,7 @@
         ctx->s = -1;
         return -1;
     }
+#endif    
 #endif
 
     return 0;
@@ -1115,7 +1120,7 @@
 static void _modbus_rtu_close(modbus_t *ctx)
 {
     /* Restore line settings and close file descriptor in RTU mode */
-    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+    //modbus_rtu_t *ctx_rtu = ctx->backend_data;
 
 #if defined(_WIN32)
     /* Revert settings */
@@ -1130,13 +1135,15 @@
     }
 #else
     if (ctx->s != -1) {
-        tcsetattr(ctx->s, TCSANOW, &ctx_rtu->old_tios);
+        //tcsetattr(ctx->s, TCSANOW, &ctx_rtu->old_tios);
         close(ctx->s);
         ctx->s = -1;
     }
 #endif
 }
 
+#define RTSER_RTIOC_FLUSH_SERIAL _IOR(RTIOC_TYPE_SERIAL, 0x07, int)
+
 static int _modbus_rtu_flush(modbus_t *ctx)
 {
 #if defined(_WIN32)
@@ -1144,13 +1151,36 @@
     ctx_rtu->w_ser.n_bytes = 0;
     return (PurgeComm(ctx_rtu->w_ser.fd, PURGE_RXCLEAR) == FALSE);
 #else
-    return tcflush(ctx->s, TCIOFLUSH);
+    int ret = 0;
+
+	ret = ioctl(ctx->s, RTSER_RTIOC_FLUSH_SERIAL);
+	if (ret)
+        fprintf(stderr, "RTSER_RTIOC_FLUSH_SERIAL failed\n");
+    return ret;//tcflush(ctx->s, TCIOFLUSH);
 #endif
 }
 
 static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset,
                               struct timeval *tv, int length_to_read)
 {
+#if 1
+    int ret = 0;
+	struct rtser_config	config = {0};
+
+	config.config_mask = RTSER_SET_TIMEOUT_RX;
+    if(tv == NULL)
+	    config.rx_timeout = 0;
+    else
+	    config.rx_timeout = tv->tv_sec * 1000000000 + tv->tv_usec * 1000;
+    
+	ret = ioctl(ctx->s, RTSER_RTIOC_SET_CONFIG, &config);
+	if (ret)
+    {
+        fprintf(stderr, "RTSER_RTIOC_SET_CONFIG set timeout failed\n");
+		return -1;
+	}
+    return 0;
+#else
     int s_rc;
 #if defined(_WIN32)
     s_rc = win32_ser_select(&((modbus_rtu_t *)ctx->backend_data)->w_ser,
@@ -1185,6 +1215,7 @@
 #endif
 
     return s_rc;
+#endif
 }
 
 static void _modbus_rtu_free(modbus_t *ctx) {
diff -ru libmodbus-xeno-3.1.6-src/tests/Makefile.am libmodbus-xeno-3.1.6/tests/Makefile.am
--- libmodbus-xeno-3.1.6-src/tests/Makefile.am	2019-08-01 05:38:15.000000000 +0800
+++ libmodbus-xeno-3.1.6/tests/Makefile.am	2022-02-21 15:50:10.391427643 +0800
@@ -11,7 +11,7 @@
 	version
 
 common_ldflags = \
-	$(top_builddir)/src/libmodbus.la
+	$(top_builddir)/src/libmodbus.la /usr/xenomai/lib/xenomai/bootstrap.o 
 
 bandwidth_server_one_SOURCES = bandwidth-server-one.c
 bandwidth_server_one_LDADD = $(common_ldflags)
@@ -44,7 +44,7 @@
     -I${top_srcdir}/src \
     -I${top_builddir}/src
 
-AM_CFLAGS = ${my_CFLAGS}
+AM_CFLAGS = ${my_CFLAGS} -I/usr/xenomai/include/trank -D__XENO_COMPAT__ -I/usr/xenomai/include/cobalt -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__COBALT__ -I/usr/xenomai/include/alchemy  -Wl,--no-as-needed -Wl,@/usr/xenomai/lib/cobalt.wrappers -Wl,@/usr/xenomai/lib/modechk.wrappers -Wl,--wrap=main -Wl,--dynamic-list=/usr/xenomai/lib/dynlist.ld -L/usr/xenomai/lib -lcobalt -lmodechk -lpthread -lrt
 
 CLEANFILES = *~ *.log
 
diff -ru libmodbus-xeno-3.1.6-src/tests/Makefile.in libmodbus-xeno-3.1.6/tests/Makefile.in
--- libmodbus-xeno-3.1.6-src/tests/Makefile.in	2019-08-01 05:38:46.000000000 +0800
+++ libmodbus-xeno-3.1.6/tests/Makefile.in	2022-02-21 16:01:32.234395857 +0800
@@ -554,7 +554,7 @@
 top_srcdir = @top_srcdir@
 EXTRA_DIST = README.md unit-tests.sh
 common_ldflags = \
-	$(top_builddir)/src/libmodbus.la
+	$(top_builddir)/src/libmodbus.la /usr/xenomai/lib/xenomai/bootstrap.o 
 
 bandwidth_server_one_SOURCES = bandwidth-server-one.c
 bandwidth_server_one_LDADD = $(common_ldflags)
@@ -579,7 +579,7 @@
     -I${top_srcdir}/src \
     -I${top_builddir}/src
 
-AM_CFLAGS = ${my_CFLAGS}
+AM_CFLAGS = ${my_CFLAGS} -I/usr/xenomai/include/trank -D__XENO_COMPAT__ -I/usr/xenomai/include/cobalt -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__COBALT__ -I/usr/xenomai/include/alchemy  -Wl,--no-as-needed -Wl,@/usr/xenomai/lib/cobalt.wrappers -Wl,@/usr/xenomai/lib/modechk.wrappers -Wl,--wrap=main -Wl,--dynamic-list=/usr/xenomai/lib/dynlist.ld -L/usr/xenomai/lib -lcobalt -lmodechk -lpthread -lrt
 CLEANFILES = *~ *.log
 noinst_SCRIPTS = unit-tests.sh
 TESTS = ./unit-tests.sh
diff -ru libmodbus-xeno-3.1.6-src/tests/unit-test-client.c libmodbus-xeno-3.1.6/tests/unit-test-client.c
--- libmodbus-xeno-3.1.6-src/tests/unit-test-client.c	2019-08-01 05:38:15.000000000 +0800
+++ libmodbus-xeno-3.1.6/tests/unit-test-client.c	2022-02-21 15:50:10.411427859 +0800
@@ -87,7 +87,8 @@
     } else if (use_backend == TCP_PI) {
         ctx = modbus_new_tcp_pi("::1", "1502");
     } else {
-        ctx = modbus_new_rtu("/dev/ttyUSB1", 115200, 'N', 8, 1);
+        ctx = modbus_new_rtu("/dev/rtdm/rtser0", 115200, 'N', 8, 1);
+        //ctx = modbus_new_rtu("/dev/ttyAMA0", 115200, 'N', 8, 1);
     }
     if (ctx == NULL) {
         fprintf(stderr, "Unable to allocate libmodbus context\n");
@@ -110,6 +111,8 @@
     }
     modbus_get_response_timeout(ctx, &new_response_to_sec, &new_response_to_usec);
 
+    modbus_set_response_timeout(ctx,1,0);
+    
     printf("** UNIT TESTING **\n");
 
     printf("1/1 No response timeout modification on connect: ");
@@ -237,6 +240,7 @@
     /* Write registers to zero from tab_rp_registers and store read registers
        into tab_rp_registers. So the read registers must set to 0, except the
        first one because there is an offset of 1 register on write. */
+    modbus_flush(ctx);
     rc = modbus_write_and_read_registers(ctx,
                                          UT_REGISTERS_ADDRESS + 1,
                                          UT_REGISTERS_NB - 1,
diff -ru libmodbus-xeno-3.1.6-src/tests/unit-test-server.c libmodbus-xeno-3.1.6/tests/unit-test-server.c
--- libmodbus-xeno-3.1.6-src/tests/unit-test-server.c	2019-08-01 05:38:15.000000000 +0800
+++ libmodbus-xeno-3.1.6/tests/unit-test-server.c	2022-02-21 15:50:10.411427859 +0800
@@ -63,7 +63,7 @@
         ctx = modbus_new_tcp_pi("::0", "1502");
         query = malloc(MODBUS_TCP_MAX_ADU_LENGTH);
     } else {
-        ctx = modbus_new_rtu("/dev/ttyUSB0", 115200, 'N', 8, 1);
+        ctx = modbus_new_rtu("/dev/rtdm/rtser0", 115200, 'N', 8, 1);
         modbus_set_slave(ctx, SERVER_ID);
         query = malloc(MODBUS_RTU_MAX_ADU_LENGTH);
     }
@@ -110,6 +110,7 @@
         }
     }
 
+    printf("loop test start ...\n");
     for (;;) {
         do {
             rc = modbus_receive(ctx, query);