diff --git a/userspace/tools/Makefile b/userspace/tools/Makefile
index acb65105fe7ec5faec55c252fea10125fb5f96c6..247fd11a8be785e896543d5e5c0af24b2882ca66 100644
--- a/userspace/tools/Makefile
+++ b/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)
diff --git a/userspace/tools/nmea/serial.h b/userspace/tools/nmea/serial.h
index 483f583a5c31fbdf2402e130f3bbf61786748d97..31e9214f834376ca573af9ea94beef637e73068d 100644
--- a/userspace/tools/nmea/serial.h
+++ b/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);
diff --git a/userspace/tools/nmea/serial_linux.c b/userspace/tools/nmea/serial_linux.c
index 83f1374997a118694eefb76e302d4a3b22f2a48c..14c71aff69577e13120c997f34c7b361cd518ad4 100644
--- a/userspace/tools/nmea/serial_linux.c
+++ b/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;
diff --git a/userspace/tools/nmea/wr_nmea.c b/userspace/tools/nmea/wr_nmea.c
index afbeaf0591518ea86483b25793dad015905e09b1..03a789c70b4497bb89f418ae80d9a474450b7c4e 100644
--- a/userspace/tools/nmea/wr_nmea.c
+++ b/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,
diff --git a/userspace/tools/wr_date.c b/userspace/tools/wr_date.c
index 1876e927e393c4c7a6a2e6e250c2f0c8ed8a119f..ede197e4ee1171a1d72166740e15acf3de4ac462 100644
--- a/userspace/tools/wr_date.c
+++ b/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 */