1 files changed, 175 insertions, 6 deletions

diff --git a/meta-facebook/meta-wedge/recipes-wedge/rackmon/rackmon/modbus.c b/meta-facebook/meta-wedge/recipes-wedge/rackmon/rackmon/modbus.c
index 46618a9..8fb0835 100644
--- a/meta-facebook/meta-wedge/recipes-wedge/rackmon/rackmon/modbus.c
+++ b/meta-facebook/meta-wedge/recipes-wedge/rackmon/rackmon/modbus.c
@@ -16,6 +16,7 @@
  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include <time.h>
 #include "modbus.h"
 #include <termios.h>
 #include <unistd.h>
@@ -26,9 +27,15 @@
 #include <string.h>
 #include <errno.h>
 #include <sys/ioctl.h>
+#include <sched.h>
+#include <pthread.h>
 
-static int loops = 0;
-void waitfd(int fd) {
+int verbose = 0;
+
+#define TIOCSERWAITTEMT 0x5499
+int waitfd(int fd) {
+  int loops = 0;
+  ioctl(fd, TIOCSERWAITTEMT, DEFAULT_GPIO);
   while(1) {
     int lsr;
     int ret = ioctl(fd, TIOCSERGETLSR, &lsr);
@@ -37,8 +44,10 @@ void waitfd(int fd) {
       break;
     }
     if(lsr & TIOCSER_TEMT) break;
+    // never should hit this with new ioctl
     loops++;
   }
+  return loops;
 }
 
 void gpio_on(int fd) {
@@ -60,8 +69,7 @@ void decode_hex_in_place(char* buf, size_t* len) {
 
 void append_modbus_crc16(char* buf, size_t* len) {
   uint16_t crc = modbus_crc16(buf, *len);
-  if (verbose)
-    fprintf(stderr, "[*] Append Modbus CRC16 %04x\n", crc);
+  dbg("[*] Append Modbus CRC16 %04x\n", crc);
   buf[(*len)++] = crc >> 8;
   buf[(*len)++] = crc & 0x00FF;
 }
@@ -78,7 +86,7 @@ size_t read_wait(int fd, char* dst, size_t maxlen, int mdelay_us) {
   size_t read_size = 0;
   size_t pos = 0;
   memset(dst, 0, maxlen);
-  for(;;) {
+  while(pos < maxlen) {
     FD_ZERO(&fdset);
     FD_SET(fd, &fdset);
     timeout.tv_sec = 0;
@@ -95,10 +103,11 @@ size_t read_wait(int fd, char* dst, size_t maxlen, int mdelay_us) {
       fprintf(stderr, "read error: %s\n", strerror(errno));
       exit(1);
     }
-    if((pos + read_size) < maxlen) {
+    if((pos + read_size) <= maxlen) {
       memcpy(dst + pos, read_buf, read_size);
       pos += read_size;
     } else {
+      return pos;
       fprintf(stderr, "Response buffer overflowed!\n");
     }
   }
@@ -181,3 +190,163 @@ uint16_t modbus_crc16(char* buffer, size_t buffer_length) {
 
     return (crc_hi << 8 | crc_lo);
 }
+
+
+double ts_diff (struct timespec* begin, struct timespec* end) {
+  return 1000.0 * (end->tv_sec) + (1e-6 * end->tv_nsec)
+    - (1000.0 * (begin->tv_sec) + (1e-6 * begin->tv_nsec));
+}
+
+static long success = 0;
+static long crcfail = 0;
+static long timeout = 0;
+static long stat_wait = 0;
+
+int modbuscmd(modbus_req *req) {
+    int error = 0;
+    struct termios tio;
+    char modbus_cmd[req->cmd_len + 2];
+    size_t cmd_len = req->cmd_len;
+
+    if (verbose)
+      fprintf(stderr, "[*] Setting TTY flags!\n");
+    memset(&tio, 0, sizeof(tio));
+    // CREAD should be left *off* until we've confirmed THRE
+    // to avoid catching false character starts
+    cfsetspeed(&tio,B19200);
+    tio.c_cflag |= PARENB;
+    tio.c_cflag |= CLOCAL;
+    tio.c_cflag |= CS8;
+    tio.c_iflag |= INPCK;
+    tio.c_cc[VMIN] = 1;
+    tio.c_cc[VTIME] = 0;
+    CHECK(tcsetattr(req->tty_fd,TCSANOW,&tio));
+
+    memcpy(modbus_cmd, req->modbus_cmd, cmd_len);
+    append_modbus_crc16(modbus_cmd, &cmd_len);
+
+    // print command as sent
+    if (verbose)  {
+      fprintf(stderr, "Will send:  ");
+      print_hex(stderr, modbus_cmd, cmd_len);
+      fprintf(stderr, "\n");
+    }
+
+    dbg("[*] Writing!\n");
+
+    // hoped adding the ioctl to do the switching would have alleviated the
+    // need to do SCHED_FIFO, but we still get preempted between the write and
+    // ioctl syscalls w/o it often enough to break f/w updates.
+    struct sched_param sp;
+    sp.sched_priority = 50;
+    int policy = SCHED_FIFO;
+    CHECKP(sched, pthread_setschedparam(pthread_self(), policy, &sp));
+    // gpio on, write, wait, gpio off
+    gpio_on(req->gpio_fd);
+    struct timespec write_begin;
+    struct timespec wait_begin;
+    struct timespec wait_end;
+    struct timespec read_end;
+    clock_gettime(CLOCK_MONOTONIC_RAW, &write_begin);
+    write(req->tty_fd, modbus_cmd, cmd_len);
+    clock_gettime(CLOCK_MONOTONIC_RAW, &wait_begin);
+    int waitloops = waitfd(req->tty_fd);
+    clock_gettime(CLOCK_MONOTONIC_RAW, &wait_end);
+    gpio_off(req->gpio_fd);
+    sp.sched_priority = 0;
+    // Enable UART read
+    tio.c_cflag |= CREAD;
+    CHECK(tcsetattr(req->tty_fd,TCSANOW,&tio));
+    policy = SCHED_OTHER;
+    CHECKP(sched, pthread_setschedparam(pthread_self(), policy, &sp));
+
+    dbg("[*] waitfd loops: %d\n", waitloops);
+    dbg("[*] reading any response...\n");
+    // Read back response
+    size_t mb_pos = 0;
+    memset(req->dest_buf, 0, req->dest_limit);
+    if(req->expected_len == 0) {
+      req->expected_len = req->dest_limit;
+    }
+    if(req->expected_len > req->dest_limit) {
+      return -1;
+    }
+    mb_pos = read_wait(req->tty_fd, req->dest_buf, req->expected_len, req->timeout);
+    clock_gettime(CLOCK_MONOTONIC_RAW, &read_end);
+    req->dest_len = mb_pos;
+    if(mb_pos >= 4) {
+      uint16_t crc = modbus_crc16(req->dest_buf, mb_pos - 2);
+      dbg("Modbus response CRC: %04X\n ", crc);
+      if((req->dest_buf[mb_pos - 2] == (crc >> 8)) &&
+          (req->dest_buf[mb_pos - 1] == (crc & 0x00FF))) {
+        dbg("CRC OK!\n");
+      } else {
+        dbg("BAD CRC :(\n");
+        fprintf(stderr, "bad crc timings:");
+        fprintf(stderr, "  write: %.2f ms", ts_diff(&write_begin, &wait_begin));
+        fprintf(stderr, "  wait: %.2f ms", ts_diff(&wait_begin, &wait_end));
+        fprintf(stderr, "  read: %.2f ms\n", ts_diff(&wait_end, &read_end));
+        if(!req->scan) {
+          crcfail++;
+        }
+        if(verbose) {
+          print_hex(stderr, req->dest_buf, mb_pos);
+        }
+        return MODBUS_BAD_CRC;
+      }
+    } else {
+      fprintf(stderr, "timeout timings:");
+      fprintf(stderr, "  write: %.2f ms", ts_diff(&write_begin, &wait_begin));
+      fprintf(stderr, "  wait: %.2f ms", ts_diff(&wait_begin, &wait_end));
+      fprintf(stderr, "  wait: %d iters", waitloops);
+      fprintf(stderr, "  read: %.2f ms\n", ts_diff(&wait_end, &read_end));
+      dbg("No response :(\n");
+      if(!req->scan) {
+        timeout++;
+      }
+      return MODBUS_RESPONSE_TIMEOUT;
+    }
+
+cleanup:
+    if(error != 0) {
+      error = -1;
+      fprintf(stderr, "%s\n", strerror(errno));
+    } else {
+      //fprintf(stderr, "success, timings:");
+      //fprintf(stderr, "  write: %.2f ms", ts_diff(&write_begin, &wait_begin));
+      //fprintf(stderr, "  wait: %.2f ms", ts_diff(&wait_begin, &wait_end));
+      //fprintf(stderr, "  read: %.2f ms -- ", ts_diff(&wait_end, &read_end));
+      if(stat_wait == 0 && !req->scan) {
+        fprintf(stderr, "success: %.2f%%  crcfail %.2f%%, timeout %.2f%%\n",
+            ((double) 100.0 * success / (success + crcfail + timeout)),
+            ((double) 100.0 * crcfail / (success + crcfail + timeout)),
+            ((double) 100.0 * timeout / (success + crcfail + timeout)));
+        stat_wait = 1000;
+        fprintf(stderr, "success timings:");
+        fprintf(stderr, "  write: %.2f ms", ts_diff(&write_begin, &wait_begin));
+        fprintf(stderr, "  wait: %.2f ms", ts_diff(&wait_begin, &wait_end));
+        fprintf(stderr, "  wait: %d iters", waitloops);
+        fprintf(stderr, "  read: %.2f ms\n", ts_diff(&wait_end, &read_end));
+      } else if (!req->scan) {
+        stat_wait--;
+      }
+      if(!req->scan) {
+        success++;
+      }
+    }
+    return 0;
+}
+
+const char* modbus_strerror(int mb_err) {
+  if (mb_err < 0) {
+    mb_err = -mb_err;
+  }
+  switch(mb_err) {
+    case 4:
+      return "timed out";
+    case 5:
+      return "crc check failed";
+    default:
+      return "unknown";
+  }
+}