handle rs485 hardware echo

src/modbus-rtu-private.h

@@ -63,6 +63,7 @@ typedef struct _modbus_rtu {
 #if HAVE_DECL_TIOCSRS485
     int serial_mode;
 #endif
+    uint8_t echohw;
 #if HAVE_DECL_TIOCM_RTS
     int rts;
     int rts_delay;

src/modbus-rtu.c

@@ -267,6 +267,26 @@ static void _modbus_rtu_ioctl_rts(modbus_t *ctx, int on)
 }
 #endif
 
+ssize_t _modbus_rtu_write_n_read(modbus_t *ctx, const uint8_t *req, int req_length) {
+ ssize_t w, r, i;
+ uint8_t rb[req_length];
+
+ // Transmit
+ w = write(ctx->s, req, req_length);
+
+ // Read back written bytes if hw has echo
+ r = 0;
+ while (r < w)
+  r += read(ctx->s, rb + r, w - r);
+ if (ctx->debug) {
+  for (i = 0; i < r; ++i)
+  fprintf(stderr, "|%02X|", rb[i]);
+  fprintf(stderr, "\n");
+ }
+
+ return w;
+}
+
 static ssize_t _modbus_rtu_send(modbus_t *ctx, const uint8_t *req, int req_length)
 {
 #if defined(_WIN32)
@@ -286,7 +306,10 @@ static ssize_t _modbus_rtu_send(modbus_t *ctx, const uint8_t *req, int req_lengt
         ctx_rtu->set_rts(ctx, ctx_rtu->rts == MODBUS_RTU_RTS_UP);
         usleep(ctx_rtu->rts_delay);
 
-        size = write(ctx->s, req, req_length);
+        if(!ctx_rtu->echohw)
+            size = write(ctx->s, req, req_length);
+          else
+            size = _modbus_rtu_write_n_read(ctx, req, req_length);
 
         usleep(ctx_rtu->onebyte_time * req_length + ctx_rtu->rts_delay);
         ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
@@ -294,7 +317,14 @@ static ssize_t _modbus_rtu_send(modbus_t *ctx, const uint8_t *req, int req_lengt
         return size;
     } else {
 #endif
-        return write(ctx->s, req, req_length);
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+    ssize_t w;
+    
+    if(!ctx_rtu->echohw)    
+      w = write(ctx->s, req, req_length);
+     else
+      w = _modbus_rtu_write_n_read(ctx, req, req_length);
+    return w;
 #if HAVE_DECL_TIOCM_RTS
     }
 #endif
@@ -1112,6 +1142,27 @@ int modbus_rtu_set_rts_delay(modbus_t *ctx, int us)
     }
 }
 
+int modbus_rtu_set_echohw_mode(modbus_t* ctx, uint8_t mode) {
+ if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+ modbus_rtu_t* rtu = (modbus_rtu_t*) ctx->backend_data;
+ rtu->echohw= mode;
+ return 0;
+ }
+ /* Wrong backend and invalid mode specified */
+ errno = EINVAL;
+ return -1;
+}
+
+int modbus_rtu_get_echohw_mode(modbus_t* ctx) {
+ if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+ modbus_rtu_t* rtu = (modbus_rtu_t*) ctx->backend_data;
+ return rtu->echohw;
+ }
+ /* Wrong backend and invalid mode specified */
+ errno = EINVAL;
+ return -1;
+}
+
 static void _modbus_rtu_close(modbus_t *ctx)
 {
     /* Restore line settings and close file descriptor in RTU mode */

src/modbus-rtu.h

@@ -37,6 +37,12 @@ MODBUS_API int modbus_rtu_set_custom_rts(modbus_t *ctx, void (*set_rts) (modbus_
 MODBUS_API int modbus_rtu_set_rts_delay(modbus_t *ctx, int us);
 MODBUS_API int modbus_rtu_get_rts_delay(modbus_t *ctx);
 
+#define MODBUS_RTU_HAS_ECHOHW 1
+#define MODBUS_RTU_NO_ECHOHW 0
+
+int modbus_rtu_set_echohw_mode(modbus_t *ctx, uint8_t mode);
+int modbus_rtu_get_echohw_mode(modbus_t *ctx);
+
 MODBUS_END_DECLS
 
 #endif /* MODBUS_RTU_H */