[FEATURE: #335] userspace/tools/nmea: add timeout 10s on NMEA readouts

Signed-off-by: Adam Wujek <dev_public@wujek.eu>

userspace/tools/Makefile

@@ -90,6 +90,8 @@ wrs_dump_shmem: wrs_dump_shmem.o wrs_dump_shmem_ppsi.o time_lib.o
 wr_mon: wr_mon.o term.o  time_lib.o
 	${CC} -o $@ $^ $(LDFLAGS)
 
+nmea/wr_nmea.o: nmea/serial.h nmea/wr_nmea.h nmea/nmea.h
+nmea/nmea.o: nmea/nmea.h
 wr_date: nmea/wr_nmea.o nmea/nmea.o nmea/serial_linux.o
 wr_date: wr_date.o time_lib.o
 	${CC} -o $@ $^ $(LDFLAGS)

userspace/tools/nmea/serial.h

@@ -19,6 +19,7 @@ int serial_read(char *data, int len);
 int serial_write(char *data, int len);
 void serial_write_byte(unsigned char b);
 unsigned char serial_read_byte(void);
+char serial_read_byte_w_timeout(unsigned int *timeout_s);
 int serial_data_avail(void);
 
 void sys_delay(int msecs);

userspace/tools/nmea/serial_linux.c

@@ -96,6 +96,36 @@ int serial_read(char *data, int len)
     return nbytes;
 };
 
+/* serial_read with timeout as seconds */
+int serial_read_w_timeout(char *data, int len, unsigned int *timeout_s)
+{
+    int nbytes = 0;
+    int select_ret;
+    fd_set set;
+    struct timeval tv;
+
+    FD_ZERO(&set);
+    FD_SET(serial_fd, &set);
+
+    while (len) {
+	tv.tv_sec = *timeout_s;
+	tv.tv_usec = 0;
+
+	select_ret = select(serial_fd + 1, &set, NULL, NULL, &tv);
+
+	if (!select_ret) {
+		/* Timeout */
+		*timeout_s = 0;
+		return 0;
+	}
+	if (read(serial_fd, data, 1) == 1) {
+	    len--; data++; nbytes++;
+	}
+    }
+
+    return nbytes;
+};
+
 void serial_write_byte(unsigned char b)
 {
 #ifdef DEBUG
@@ -118,6 +148,20 @@ char serial_read_byte(void)
 
 }
 
+/* serial_read_byte with timeout as seconds */
+char serial_read_byte_w_timeout(unsigned int *timeout_s)
+{
+    char b;
+
+    serial_read_w_timeout(&b, 1, timeout_s);
+#ifdef DEBUG
+    printf("%02x ", b);
+#endif /* DEBUG */
+
+    return b;
+
+}
+
 int serial_data_avail(void)
 {
     fd_set set;

userspace/tools/nmea/wr_nmea.c

@@ -32,26 +32,71 @@ int nmea_init(struct wr_nmea *nmea, char *dev, int baud, char *fmt)
 	return 0;
 }
 
-static void read_nmea_msg(char *msgbuf, int len)
+static int read_nmea_msg(char *msgbuf, int len)
 {
 	int i = 0;
 	char c;
+	unsigned int nmea_timeout_max = 10;
+	unsigned int nmea_timeout;
+
+	while (1) {
+		if (nmea_timeout_max <= 0) {
+			/* Timeout */
+			return -1;
+		}
+
+		nmea_timeout = 1;
+		c = serial_read_byte_w_timeout(&nmea_timeout);
+		if (nmea_timeout == 0) {
+			nmea_timeout_max--;
+			continue;
+		}
+
+		/* Start of message found */
+		if (c == '$')
+		    break;
+	}
 
-	while ((c = serial_read_byte()) != '$') {};
-		*msgbuf++ = c;
-	while ((c = serial_read_byte()) != '\r' && (i++) < len - 1)
+	/* Copy start of message ('$') */
+	*msgbuf++ = c;
+
+	while (1) {
+		if (nmea_timeout_max <= 0) {
+			/* Timeout */
+			return -1;
+		}
+
+		nmea_timeout = 1;
+		c = serial_read_byte_w_timeout(&nmea_timeout);
+		if (nmea_timeout == 0) {
+			nmea_timeout_max--;
+			continue;
+		}
+
+		if (c == '\r')
+			break;
+		i++;
+		if (i >= len)
+			break;
 		*msgbuf++ = c;
+	}
+
 	*msgbuf++ = '\r';
 	*msgbuf++ = '\n';
 
-	*msgbuf++ = 0 ;
+	*msgbuf++ = 0;
+
+	return 0;
 }
 
-void read_nmea_msg_type(char *msgbuf, int len, const char *msg_type)
+int read_nmea_msg_type(char *msgbuf, int len, const char *msg_type)
 {
 	do {
-		read_nmea_msg(msgbuf, len);
+		if (read_nmea_msg(msgbuf, len) < 0)
+			return -1;
 	} while (strncmp(&msgbuf[1], msg_type, 5) != 0); //ignore starting "$"
+
+	return 0;
 }
 
 int nmea_read_tai(struct wr_nmea *nmea, int64_t *t_out)
@@ -59,11 +104,12 @@ int nmea_read_tai(struct wr_nmea *nmea, int64_t *t_out)
     char buf[1024];
 
     serial_open(nmea->dev, nmea->baud);
-    read_nmea_msg_type(buf, 1024, nmea->fmt);
+    if (read_nmea_msg_type(buf, 1024, nmea->fmt) < 0)
+	return -1;
     serial_close();
 
     if(nmea->parse(buf, strlen(buf), (nmea->utc)) < 0)
-	return -1;
+	return -2;
 
     pr_info("NMEA time: %d/%d/%d %02d:%02d:%02d.%02d\n",
 	    nmea->utc->year+1900,

userspace/tools/wr_date.c

@@ -138,19 +138,29 @@ int get_kern_leaps(void)
 
 static int wrdate_get_nmea(int64_t *t_out)
 {
-	if (nmea_read_tai(&nmea, t_out) < 0) {
-		fprintf(stderr, "wr_date: %s: error reading nmea\n", __func__);
+	int ret;
+	
+	ret = nmea_read_tai(&nmea, t_out);
+
+	if (ret == -1) {
+		fprintf(stderr, "Timeout on reading nmea\n");
+		return -1;
+	}
+
+	if (ret == -2) {
+		fprintf(stderr, "Error while parsing nmea message\n");
 		return -1;
 	}
+
 	return 0;
 }
 
-static void wrdate_gettimeofday(struct timeval *tv)
+static int wrdate_gettimeofday(struct timeval *tv)
 {
 	if (opt_nmea_en) {
-		wrdate_get_nmea((int64_t *)&tv->tv_sec);
+		return wrdate_get_nmea((int64_t *)&tv->tv_sec);
 	} else {
-		gettimeofday(tv, NULL);
+		return gettimeofday(tv, NULL);
 	}
 }
 
@@ -166,7 +176,8 @@ int wrdate_get(volatile struct PPSG_WB *pps, int tohost)
 
 	tai_offset = get_kern_leaps();
 	if (opt_not) {
-		wrdate_gettimeofday(&sw);
+		if (wrdate_gettimeofday(&sw) < 0)
+			return 1;
 		taih = 0;
 		tail = sw.tv_sec + tai_offset;
 		nsec = sw.tv_usec * 1000;
@@ -181,7 +192,8 @@ int wrdate_get(volatile struct PPSG_WB *pps, int tohost)
 		} while((tmp1 != taih) || (tmp2 != tail));
 	}
 
-	wrdate_gettimeofday(&sw);
+	if (wrdate_gettimeofday(&sw) < 0)
+		return 1;
 	tai = opt_nmea_en ? (sw.tv_sec) : (uint64_t)(taih) << 32 | tail;
 
 	/* Before printing (which takes time), set host time if so asked to */