fix checksum alg, add better debug

2 files changed, 73 insertions, 30 deletions

diff --git a/keypad.c b/keypad.c
index 23a94bb..f9d9b07 100644
--- a/keypad.c
+++ b/keypad.c
@@ -6,6 +6,7 @@
 #include <sys/types.h>
 #include <sys/socket.h>
 #include <ncurses.h>
+#include <stdarg.h>
 
 #include "util.h"
 
@@ -44,7 +45,7 @@
 
 #define GD_LCD_LINE_LEN     16
 
-#define GD_TIMEOUT      2
+#define GD_TIMEOUT      10
 
 // Packets are
 // AA FN D0 D1 D2 CK
@@ -70,6 +71,29 @@ typedef struct {
 } GD_LCD;
 
 
+WINDOW *win, *dwin = NULL;
+FILE *dfile = NULL;
+
+
+void debug_print (const char *fmt, ...)
+{
+
+  va_list va;
+
+  if (dwin) {
+    va_start (va, fmt);
+    vw_printw (dwin, fmt, va);
+    va_end (va);
+  }
+
+  if (dfile) {
+    va_start (va, fmt);
+    vfprintf (dfile, fmt, va);
+    va_end (va);
+  }
+}
+
+
 
 static void endecode (unsigned char *p, size_t l)
 {
@@ -83,22 +107,23 @@ static void endecode (unsigned char *p, size_t l)
 static unsigned char checksum (unsigned char *p, size_t l)
 {
   unsigned s = 0;
-  unsigned char q;
+  unsigned r = 0;
   unsigned i;
 
   for (i = 0; i < l; ++i)
     s += p[i];
 
   s += 0xaa;
-  q = s & 0xff;
-  q += (s >> 8);
-  q += (s >> 16);
-
-  return q;
-}
-
 
+  while (s > 0xff) {
+    r = s & 0xff;
+    r += (s >> 8);
+    r += (s >> 16);
+    s = r;
+  }
 
+  return s;
+}
 
 
 void hexdump_pkt (const char *prefix, GD_PKT *p)
@@ -108,37 +133,37 @@ void hexdump_pkt (const char *prefix, GD_PKT *p)
   size_t l = p->data_len + 2;
 
   for (i = 0; i < l; i += 16) {
-    fprintf (stderr, "%s %04x ", prefix, i);
+    debug_print ("%s %04x ", prefix, i);
 
     for (j = 0; j < 16; ++j) {
       k = i + j;
 
       if (k < l)
-        fprintf (stderr, " %02x", d[k]);
+        debug_print (" %02x", d[k]);
 
       else
-        fprintf (stderr, "   ");
+        debug_print ("   ");
 
-      if (j == 7) fprintf (stderr, " ");
+      if (j == 7) debug_print (" ");
     }
 
-    fprintf (stderr, "  ");
+    debug_print ("  ");
 
     for (j = 0; j < 16; ++j) {
       k = i + j;
 
       if (k < l) {
         if ((d[k] > 32) && (d[k] < 127))
-          fprintf (stderr, "%c", d[k]);
+          debug_print ("%c", d[k]);
         else
-          fprintf (stderr, ".");
+          debug_print (".");
       } else
-        fprintf (stderr, " ");
+        debug_print (" ");
 
-      if (j == 7) fprintf (stderr, " ");
+      if (j == 7) debug_print (" ");
     }
 
-    fprintf (stderr, "\n");
+    debug_print ("\n");
   }
 
 }
@@ -148,7 +173,7 @@ int send_pkt (int fd, GD_PKT *tx)
   unsigned char buf[GD_MAX_PKT_LEN];
   size_t len = tx->data_len + 2;
 
-  //hexdump_pkt (" >", tx);
+  hexdump_pkt (" >", tx);
 
   memcpy (buf, tx->pkt, len);
   endecode (buf, len);
@@ -177,12 +202,19 @@ int recv_pkt (int fd, GD_PKT *rx, int timeout)
 
   sum = checksum (rx->pkt, len) ;
 
-  if (rx->pkt[len] != sum) return -1;
+  if (rx->pkt[len] != sum) {
+    rx->data_len = len - 1;
+
+    hexdump_pkt ("!<", rx);
+    debug_print ("Bad checksum expected %02x got %02x\n", sum, rx->pkt[len]);
+
+    return -1;
+  }
 
   endecode (rx->pkt, len);
   rx->data_len = len - 2;
 
-  //hexdump_pkt (" <", rx);
+  hexdump_pkt (" <", rx);
 
 
   return 1;
@@ -297,8 +329,8 @@ void gd_to_ascii (unsigned char *c, size_t l)
 
   for (; l; l--, c++) {
     if (*c == 0xdb) *c = '+';
-
-    if (*c == 0xb0) *c = '-';
+    else if (*c == 0xb0) *c = '-';
+    else if ((*c > 126) || (*c < 32)) *c = '?';
   }
 
 }
@@ -520,7 +552,6 @@ int gd_init (int fd)
   return 0;
 }
 
-WINDOW *win;
 
 void curses_init (void)
 {
@@ -558,6 +589,15 @@ void curses_init (void)
   wrefresh (win);
   timeout (0);
 
+  dwin = newwin (20, 80, 16, 0);
+  box (dwin, 0, 0);
+  dwin = newwin (18, 78, 17, 1);
+  scrollok (dwin, TRUE);
+  wrefresh (dwin);
+  timeout (0);
+
+
+
 }
 
 
@@ -584,6 +624,8 @@ int gd_keypad (int fd)
   GD_LCD l;
   int ch;
 
+//  dfile = fopen ("log", "w");
+
   if (gd_init (fd)) {
     printf ("Failed to init comms with panel\n");
     return -1;
@@ -595,15 +637,16 @@ int gd_keypad (int fd)
 
   while ((ch = getch()) != 'q') {
 
-    gd_fn_poll (fd, &l);
-    curses_show (&l);
-
     if (ch == KEY_LEFT) ch = 'a';
 
     if (ch == KEY_RIGHT) ch = 'b';
 
     if (ch != ERR && !gd_fn_key_press (fd, ch, &l))
       curses_show (&l);
+    else  {
+      gd_fn_poll (fd, &l);
+      curses_show (&l);
+    }
   }
 
 
diff --git a/util.c b/util.c
index ab9afcc..e06fc62 100644
--- a/util.c
+++ b/util.c
@@ -52,8 +52,8 @@ ssize_t read_with_timeout (int fd, void *_b, size_t len, unsigned timeout)
   fd_set rfds;
   ssize_t red, ret = 0;
 
-  tv.tv_sec = timeout / 10;
-  tv.tv_usec = (timeout % 10) * 100000;
+  tv.tv_sec = timeout / 100;
+  tv.tv_usec = (timeout % 100) * 10000;
 
   while (tv.tv_sec || tv.tv_usec) {
     FD_ZERO (&rfds);