1 files changed, 36 insertions, 17 deletions

diff --git a/gm_platform/fw/tw_test.c b/gm_platform/fw/tw_test.c
index 60c6c67..eb41dbe 100644
--- a/gm_platform/fw/tw_test.c
+++ b/gm_platform/fw/tw_test.c
@@ -281,11 +281,22 @@ int main(int argc, char *argv[]) {
     if (epoll_ctl(epollfd, EPOLL_CTL_ADD, fd, &ev) < 0)
         goto epoll_err;
 
-    int current_seq = -1;
     wpacket.type = 1;
     wpacket.pid = 0;
     cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket));
     write(fd, wbuf, sizeof(wbuf));
+
+    /* FIXME begin debug code */
+    for (int i=0; i<32; i++) {
+        wpacket.type = 2;
+        wpacket.pid = packet.pid;
+        cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket));
+        write(fd, wbuf, sizeof(wbuf));
+        usleep(20);
+    }
+    /* FIXME end debug code */
+
+    int current_seq = -1;
     uint64_t local_seq = 0;
     while (23) {
         int nfds = epoll_wait(epollfd, events, MAX_EVENTS, -1);
@@ -296,6 +307,8 @@ int main(int argc, char *argv[]) {
             continue;
 
         ssize_t n = read(fd, buf+wpos, sizeof(buf)-wpos);
+        printf("--- read wpos=%d n=%ld\n", wpos, n);
+        hexdump(buf+wpos, n);
         if (n<0) {
             if (errno == EAGAIN || errno == EINTR)
                 continue;
@@ -303,17 +316,17 @@ int main(int argc, char *argv[]) {
             fprintf(stderr, "Error reading from port: %s\n", strerror(errno));
             goto loop_err;
         }
-        printf("--- debug: read n=%d bytes at wpos=%d\n", n, wpos);
-        fflush(stdout);
+        //printf("--- debug: read n=%d bytes at wpos=%d\n", n, wpos);
+        //fflush(stdout);
         wpos += n;
 
         while (23) {
             void *first_nul = memchr(buf, 0, wpos) ;
+            ssize_t first_nul_offx = first_nul - (void*)buf;
+            ssize_t remaining = wpos - first_nul_offx;
 
             if (!in_sync) {
                 if (first_nul) {
-                    ssize_t first_nul_offx = first_nul - (void*)buf;
-                    ssize_t remaining = wpos - first_nul_offx;
                     memmove(buf, first_nul+1, remaining-1);
                     wpos = remaining-1;
                     in_sync = 1;
@@ -328,6 +341,9 @@ int main(int argc, char *argv[]) {
             if (!first_nul)
                 break;
 
+            printf("--- debug: first_nul=%p (idx=%ld) wpos=%d remaining=%ld\n", first_nul, first_nul_offx, wpos, remaining);
+            hexdump(buf, 80);
+
             int rc = cobs_decode((char *)&packet, sizeof(packet), buf, wpos);
             if (rc < 0) {
                 printf("Framing error: rc=%d\n", rc);
@@ -344,21 +360,26 @@ int main(int argc, char *argv[]) {
                 }
             }
 
+            bool error = false;
             /* Check CRC */
             if (our_crc != packet.crc) {
                 printf("CRC mismatch: seq=%d packet=%08x computed=%08x\n", packet.pid, packet.crc, our_crc);
-                goto it_err;
+                error = true;
             }
 
             /* Check device sequence number */
             int last_seq = current_seq;
             int predicted_seq = (last_seq+1) % 0xffff;
-            current_seq = packet.seq;
+            if (!error)
+                current_seq = packet.seq;
             if (last_seq >= 0 && packet.seq != predicted_seq) {
                 printf("SEQ mismatch: packet=%d computed=%d\n", packet.seq, predicted_seq);
-                goto it_err;
+                error = true;
             }
 
+            if (error)
+                goto it_err;
+
             /* Write to database */
             struct timespec ts;
             if (clock_gettime(CLOCK_REALTIME, &ts)) {
@@ -394,23 +415,21 @@ int main(int argc, char *argv[]) {
 
             printf("OK: seq=%d crc=%08x\n", current_seq, packet.crc);
 
-it_err:
-            /* FIXME don't send acks in case of error */
             /* send ACK reply */
             wpacket.type = 2;
             wpacket.pid = packet.pid;
             cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket));
             write(fd, wbuf, sizeof(wbuf));
 
+it_err:
             /* Fixup buffer for next iteration */
-            ssize_t first_nul_offx = first_nul - (void*)buf;
-            ssize_t remaining = wpos - first_nul_offx;
-            printf("--- debug: first_nul=%p (idx=%d) wpos=%d remaining=%d\n", first_nul, first_nul_offx, wpos, remaining);
-            hexdump(buf, 80);
-            printf(" ---memmove(buf=%p, first_nul+1=%p, remaining-1=%d);-->\n", buf, first_nul+1, remaining-1);
-            memmove(buf, first_nul+1, remaining-1);
-            hexdump(buf, 80);
+            if (remaining-1 > 0) {
+                printf(" ---memmove(buf=%p, first_nul+1=%p, remaining-1=%ld);-->\n", buf, first_nul+1, remaining-1);
+                memmove(buf, first_nul+1, remaining-1);
+            }
+            //hexdump(buf, 80);
             fflush(stdout);
+            printf("--- continuing wpos=%d->%d\n", wpos, (int)(remaining-1));
             wpos = remaining-1;
         }
     }