empty input queue on CRC error

src/modbus-rtu.c

@@ -411,6 +411,35 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
     if (crc_calculated == crc_received) {
         return msg_length;
     } else {
+        modbus_rtu_t *ctx_rtu = ctx->backend_data;    
+        
+        if(ctx_rtu->echohw) {/* on active echomode empty receiver on crc error */
+            int rc;
+            fd_set rfds;
+            struct timeval tv;    
+        
+            /* Add a file descriptor to the set */
+            FD_ZERO(&rfds);
+            FD_SET(ctx->s, &rfds);
+            
+            if (ctx->debug) {
+                fprintf(stderr, "ERROR CRC received on ECHOHW empty input tv.tv_sec=%d tv.tv_usec=%d\n",
+                        (int)ctx->response_timeout.tv_sec,
+                        (int)ctx->response_timeout.tv_usec);
+                fflush(stderr);                    
+            }
+        
+            do {
+               unsigned char cc[256]; /* dummy buffer für cleaning input */
+           
+               tv.tv_sec = ctx->response_timeout.tv_sec;
+               tv.tv_usec = ctx->response_timeout.tv_usec;
+
+               rc = ctx->backend->select(ctx, &rfds, &tv, sizeof(cc));
+               if(rc > 0) ctx->backend->recv(ctx, msg + msg_length, rc);           
+               } while(rc > 0); /* read until transfer empty */
+            }
+          
         if (ctx->debug) {
             fprintf(stderr, "ERROR CRC received 0x%0X != CRC calculated 0x%0X\n",
                     crc_received, crc_calculated);