diff --git a/userspace/wrsw_hal/Makefile b/userspace/wrsw_hal/Makefile
index 8e5162e549bbde485e31e6dd91b40ca702e62188..aed3ad96a16e0dc65f07b1f1c6c7d5c4f23d06e1 100644
--- a/userspace/wrsw_hal/Makefile
+++ b/userspace/wrsw_hal/Makefile
@@ -1,7 +1,7 @@
CC = $(CROSS_COMPILE)gcc
-OBJS = hal_exports.o hal_main.o hal_ports.o hal_config.o rt_client.o hal_timing.o
+OBJS = hal_exports.o hal_main.o hal_ports.o hal_config.o rt_client.o hal_timing.o gps_resync/serial_linux.o gps_resync/nmea.o gps_resync/gps_resync.o
BINARY = wrsw_hal
@@ -16,7 +16,7 @@ CFLAGS = -g -Wall -DDEBUG -DRT_CLIENT\
LDFLAGS = -L$(WR_INSTALL_ROOT)/lib -L../3rdparty/lib -L$(WR_LIB) \
-L../libswitchhw -L../mini-rpc \
- -lminipc -llua -lm -ldl -lswitchhw
+ -lminipc -llua -lm -ldl -lswitchhw
all: $(BINARY)
diff --git a/userspace/wrsw_hal/gps_resync/gps_resync.c b/userspace/wrsw_hal/gps_resync/gps_resync.c
new file mode 100644
index 0000000000000000000000000000000000000000..e07d7a7f63ffbd3efd309bbe93f83cd2099ea2ef
--- /dev/null
+++ b/userspace/wrsw_hal/gps_resync/gps_resync.c
@@ -0,0 +1,60 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <time.h>
+#include <unistd.h>
+
+
+#include "serial.h"
+#include "nmea.h"
+
+#include <hw/switch_hw.h>
+
+void read_nmea_msg(char *msgbuf, int len)
+{
+ int i=0;
+ char c;
+
+ while((c = serial_read_byte()) != '$');
+ *msgbuf++ = c;
+ while((c = serial_read_byte()) != '\r' && (i++) < len-1)
+ *msgbuf++ = c;
+ *msgbuf++ = '\r';
+ *msgbuf++ = '\n';
+
+ *msgbuf++ = 0 ;
+}
+
+static int nmea_read_tai(const char *dev_name, int64_t *t_out)
+{
+ char buf[1024];
+ nmea_gprmc_t gprmc;
+
+ serial_open((char *)dev_name, 115200);
+ read_nmea_msg(buf, 1024);
+ serial_close();
+
+ if(nmea_parse_gprmc(buf, strlen(buf), &gprmc) < 0)
+ return -1;
+
+
+ TRACE(TRACE_INFO, "NMEA time: %d/%d/%d %02d:%02d:%02d.%02d\n", gprmc.utc.year + 1900,gprmc.utc.mon+1,gprmc.utc.day,gprmc.utc.hour, gprmc.utc.min, gprmc.utc.sec,gprmc.utc.hsec);
+
+ *t_out = nmea_time_to_tai(gprmc.utc);
+
+ return 0;
+}
+
+void nmea_resync_ppsgen(const char *dev_name)
+{
+ uint32_t utc, nsec;
+ int64_t tai;
+
+ nmea_read_tai("/dev/ttyS2", &tai);
+ shw_pps_gen_read_time( &utc, &nsec);
+
+ shw_pps_gen_adjust_utc(tai - utc);
+
+ while(!shw_pps_gen_busy()) usleep(100000);
+}
+
diff --git a/userspace/wrsw_hal/gps_resync/gps_resync.h b/userspace/wrsw_hal/gps_resync/gps_resync.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7ca94930878306bef66bf9ca5eb2670f9511795
--- /dev/null
+++ b/userspace/wrsw_hal/gps_resync/gps_resync.h
@@ -0,0 +1,9 @@
+#ifndef __NMEA_RESYNC_H
+#define __NMEA_RESYNC_H
+
+#include <time.h>
+
+int nmea_get_unix_seconds(const char *dev_name, time_t *t_out);
+void nmea_resync_ppsgen(const char *dev_name);
+
+#endif
diff --git a/userspace/wrsw_hal/gps_resync/nmea.c b/userspace/wrsw_hal/gps_resync/nmea.c
new file mode 100644
index 0000000000000000000000000000000000000000..8f86b7960e223288331f9cdfe51594792a465f10
--- /dev/null
+++ b/userspace/wrsw_hal/gps_resync/nmea.c
@@ -0,0 +1,355 @@
+/*
+ *
+ * NMEA library
+ * URL: http://nmea.sourceforge.net
+ * Author: Tim (xtimor@gmail.com)
+ * Licence: http://www.gnu.org/licenses/lgpl.html
+ *
+ * Modified for use in wrsw_hal by Tomasz Wlostowski/CERN
+ */
+
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+#include <ctype.h>
+#include <string.h>
+#include <limits.h>
+
+#include "nmea.h"
+
+#define NMEA_TOKS_COMPARE (1)
+#define NMEA_TOKS_PERCENT (2)
+#define NMEA_TOKS_WIDTH (3)
+#define NMEA_TOKS_TYPE (4)
+
+#define NMEA_CONVSTR_BUF 128
+#define NMEA_TIMEPARSE_BUF 128
+
+/**
+ * \brief Calculate control sum of binary buffer
+ */
+static int nmea_calc_crc(const char *buff, int buff_sz)
+{
+ int chsum = 0,
+ it;
+
+ for(it = 0; it < buff_sz; ++it)
+ chsum ^= (int)buff[it];
+
+ return chsum;
+}
+
+/**
+ * \brief Convert string to number
+ */
+static int nmea_atoi(const char *str, int str_sz, int radix)
+{
+ char *tmp_ptr;
+ char buff[NMEA_CONVSTR_BUF];
+ int res = 0;
+
+ if(str_sz < NMEA_CONVSTR_BUF)
+ {
+ memcpy(&buff[0], str, str_sz);
+ buff[str_sz] = '\0';
+ res = strtol(&buff[0], &tmp_ptr, radix);
+ }
+
+ return res;
+}
+
+/**
+ * \brief Convert string to fraction number
+ */
+static double nmea_atof(const char *str, int str_sz)
+{
+ char *tmp_ptr;
+ char buff[NMEA_CONVSTR_BUF];
+ double res = 0;
+
+ if(str_sz < NMEA_CONVSTR_BUF)
+ {
+ memcpy(&buff[0], str, str_sz);
+ buff[str_sz] = '\0';
+ res = strtod(&buff[0], &tmp_ptr);
+ }
+
+ return res;
+}
+
+/**
+ * \brief Formating string (like standart printf) with CRC tail (*CRC)
+ */
+static int nmea_printf(char *buff, int buff_sz, const char *format, ...)
+{
+ int retval, add = 0;
+ va_list arg_ptr;
+
+ if(buff_sz <= 0)
+ return 0;
+
+ va_start(arg_ptr, format);
+
+ retval = vsnprintf(buff, buff_sz, format, arg_ptr);
+
+ if(retval > 0)
+ {
+ add =snprintf(
+ buff + retval, buff_sz - retval, "*%02x\r\n",
+ nmea_calc_crc(buff + 1, retval - 1));
+ }
+
+ retval += add;
+
+ if(retval < 0 || retval > buff_sz)
+ {
+ memset(buff, ' ', buff_sz);
+ retval = buff_sz;
+ }
+
+ va_end(arg_ptr);
+
+ return retval;
+}
+
+/**
+ * \brief Analyse string (specificate for NMEA sentences)
+ */
+static int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
+{
+ const char *beg_tok;
+ const char *end_buf = buff + buff_sz;
+
+ va_list arg_ptr;
+ int tok_type = NMEA_TOKS_COMPARE;
+ int width = 0;
+ const char *beg_fmt = 0;
+ int snum = 0, unum = 0;
+
+ int tok_count = 0;
+ void *parg_target;
+
+ va_start(arg_ptr, format);
+
+ for(; *format && buff < end_buf; ++format)
+ {
+ switch(tok_type)
+ {
+ case NMEA_TOKS_COMPARE:
+ if('%' == *format)
+ tok_type = NMEA_TOKS_PERCENT;
+ else if(*buff++ != *format)
+ goto fail;
+ break;
+ case NMEA_TOKS_PERCENT:
+ width = 0;
+ beg_fmt = format;
+ tok_type = NMEA_TOKS_WIDTH;
+ case NMEA_TOKS_WIDTH:
+ if(isdigit(*format))
+ break;
+ {
+ tok_type = NMEA_TOKS_TYPE;
+ if(format > beg_fmt)
+ width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
+ }
+ case NMEA_TOKS_TYPE:
+ beg_tok = buff;
+
+ if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
+ width = 1;
+
+ if(width)
+ {
+ if(buff + width <= end_buf)
+ buff += width;
+ else
+ goto fail;
+ }
+ else
+ {
+ if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
+ buff = end_buf;
+ }
+
+ if(buff > end_buf)
+ goto fail;
+
+ tok_type = NMEA_TOKS_COMPARE;
+ tok_count++;
+
+ parg_target = 0; width = (int)(buff - beg_tok);
+
+ switch(*format)
+ {
+ case 'c':
+ case 'C':
+ parg_target = (void *)va_arg(arg_ptr, char *);
+ if(width && 0 != (parg_target))
+ *((char *)parg_target) = *beg_tok;
+ break;
+ case 's':
+ case 'S':
+ parg_target = (void *)va_arg(arg_ptr, char *);
+ if(width && 0 != (parg_target))
+ {
+ memcpy(parg_target, beg_tok, width);
+ ((char *)parg_target)[width] = '\0';
+ }
+ break;
+ case 'f':
+ case 'g':
+ case 'G':
+ case 'e':
+ case 'E':
+ parg_target = (void *)va_arg(arg_ptr, double *);
+ if(width && 0 != (parg_target))
+ *((double *)parg_target) = nmea_atof(beg_tok, width);
+ break;
+ };
+
+ if(parg_target)
+ break;
+ if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
+ break;
+ if(!width)
+ break;
+
+ switch(*format)
+ {
+ case 'd':
+ case 'i':
+ snum = nmea_atoi(beg_tok, width, 10);
+ memcpy(parg_target, &snum, sizeof(int));
+ break;
+ case 'u':
+ unum = nmea_atoi(beg_tok, width, 10);
+ memcpy(parg_target, &unum, sizeof(unsigned int));
+ break;
+ case 'x':
+ case 'X':
+ unum = nmea_atoi(beg_tok, width, 16);
+ memcpy(parg_target, &unum, sizeof(unsigned int));
+ break;
+ case 'o':
+ unum = nmea_atoi(beg_tok, width, 8);
+ memcpy(parg_target, &unum, sizeof(unsigned int));
+ break;
+ default:
+ goto fail;
+ };
+
+ break;
+ };
+ }
+
+fail:
+
+ va_end(arg_ptr);
+
+ return tok_count;
+}
+
+static int _nmea_parse_time(const char *buff, int buff_sz, nmea_time_t *res)
+{
+ int success = 0;
+
+ switch(buff_sz)
+ {
+ case sizeof("hhmmss") - 1:
+ success = (3 == nmea_scanf(buff, buff_sz,
+ "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec)
+ ));
+ break;
+ case sizeof("hhmmss.s") - 1:
+ case sizeof("hhmmss.ss") - 1:
+ case sizeof("hhmmss.sss") - 1:
+ success = (4 == nmea_scanf(buff, buff_sz,
+ "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec)
+ ));
+ break;
+ default:
+ success = 0;
+ break;
+ }
+
+ return (success?0:-1);
+}
+
+/**
+ * \brief Parse RMC packet from buffer.
+ * @param buff a constant character pointer of packet buffer.
+ * @param buff_sz buffer size.
+ * @param pack a pointer of packet which will filled by function.
+ * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
+ */
+
+int nmea_parse_gprmc(const char *buff, int buff_sz, nmea_gprmc_t *pack)
+{
+ int nsen;
+ char time_buff[NMEA_TIMEPARSE_BUF];
+
+ assert(buff && pack);
+
+ memset(pack, 0, sizeof(nmea_gprmc_t));
+
+ nsen = nmea_scanf(buff, buff_sz,
+ "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
+ &(time_buff[0]),
+ &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
+ &(pack->speed), &(pack->direction),
+ &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
+ &(pack->declination), &(pack->declin_ew), &(pack->mode));
+
+ if(nsen != 13 && nsen != 14)
+ {
+ return 0;
+ }
+
+ if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
+ {
+ return 0;
+ }
+
+ if(pack->utc.year < 90)
+ pack->utc.year += 100;
+ pack->utc.mon -= 1;
+
+ return 1;
+}
+
+int64_t nmea_time_to_tai(nmea_time_t t)
+{
+ short month, year;
+ int64_t result;
+ const int m_to_d[12] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
+
+ month = t.mon;
+ year = t.year + month / 12 + 1900;
+ month %= 12;
+ if (month < 0)
+ {
+ year -= 1;
+ month += 12;
+ }
+ result = (year - 1970) * 365 + (year - 1969) / 4 + m_to_d[month];
+ result = (year - 1970) * 365 + m_to_d[month];
+
+ if (month <= 1)
+ year -= 1;
+
+ result += (year - 1968) / 4;
+ result -= (year - 1900) / 100;
+ result += (year - 1600) / 400;
+ result += t.day;
+ result -= 1;
+ result *= 24;
+ result += t.hour;
+ result *= 60;
+ result += t.min;
+ result *= 60;
+ result += t.sec;
+
+ return(result + 19LL); /* TAI = GPS + 19 s */
+}
diff --git a/userspace/wrsw_hal/gps_resync/nmea.h b/userspace/wrsw_hal/gps_resync/nmea.h
new file mode 100644
index 0000000000000000000000000000000000000000..158c29777a14957abc889fdc976798b4e0c3da69
--- /dev/null
+++ b/userspace/wrsw_hal/gps_resync/nmea.h
@@ -0,0 +1,51 @@
+/*
+ *
+ * NMEA library
+ * URL: http://nmea.sourceforge.net
+ * Author: Tim (xtimor@gmail.com)
+ * Licence: http://www.gnu.org/licenses/lgpl.html
+ * $Id: time.h 4 2007-08-27 13:11:03Z xtimor $
+ *
+ * Stripped-down version for use in wrsw_hal by Tomasz Wlostowski/CERN.
+ */
+
+/*! \file */
+
+#ifndef __NMEA_H__
+#define __NMEA_H__
+
+typedef struct
+{
+ int year; /**< Years since 1900 */
+ int mon; /**< Months since January - [0,11] */
+ int day; /**< Day of the month - [1,31] */
+ int hour; /**< Hours since midnight - [0,23] */
+ int min; /**< Minutes after the hour - [0,59] */
+ int sec; /**< Seconds after the minute - [0,59] */
+ int hsec; /**< Hundredth part of second - [0,99] */
+
+} nmea_time_t;
+
+/**
+ * RMC packet information structure (Recommended Minimum sentence C)
+ */
+typedef struct
+{
+ nmea_time_t utc; /**< UTC of position */
+ char status; /**< Status (A = active or V = void) */
+ double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */
+ char ns; /**< [N]orth or [S]outh */
+ double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */
+ char ew; /**< [E]ast or [W]est */
+ double speed; /**< Speed over the ground in knots */
+ double direction; /**< Track angle in degrees True */
+ double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
+ char declin_ew; /**< [E]ast or [W]est */
+ char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */
+
+} nmea_gprmc_t;
+
+int nmea_parse_gprmc(const char *buff, int buff_sz, nmea_gprmc_t *pack);
+int64_t nmea_time_to_tai(nmea_time_t t);
+
+#endif /* __NMEA_H__ */
diff --git a/userspace/wrsw_hal/gps_resync/serial.h b/userspace/wrsw_hal/gps_resync/serial.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe16f2804fb80b017d44ebefbfd550755d798c47
--- /dev/null
+++ b/userspace/wrsw_hal/gps_resync/serial.h
@@ -0,0 +1,27 @@
+/* SAM7Sisp - alternatywny bootloader dla mikrokontrolerów AT91SAM7
+
+ (c) Tomasz Wlostowski 2006
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+*/
+
+
+#ifndef __SERIAL_H
+#define __SERIAL_H
+
+int serial_open(char *dev_name, int speed);
+void serial_close();
+void serial_set_dtr(int s);
+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();
+int serial_data_avail();
+
+void sys_delay(int msecs);
+unsigned int sys_get_clock_usec();
+
+#endif
diff --git a/userspace/wrsw_hal/gps_resync/serial_linux.c b/userspace/wrsw_hal/gps_resync/serial_linux.c
new file mode 100644
index 0000000000000000000000000000000000000000..8611dd8c9e2e9603f7d5fe9216fc69c72126203d
--- /dev/null
+++ b/userspace/wrsw_hal/gps_resync/serial_linux.c
@@ -0,0 +1,140 @@
+/* SAM7Sisp - alternatywny bootloader dla mikrokontrolerów AT91SAM7
+
+ (c) Tomasz Wlostowski 2006
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+*/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <asm/ioctls.h>
+#include <string.h>
+#include <termios.h>
+#include <fcntl.h>
+
+static int serial_fd;
+
+void serial_set_dtr(int s) // pin 4
+{
+ unsigned int v;
+ if (serial_fd<0) return;
+ ioctl(serial_fd,TIOCMGET,&v);
+ if (s) v|=TIOCM_DTR; else v&=~TIOCM_DTR;
+ ioctl(serial_fd,TIOCMSET,&v);
+}
+
+int serial_open(char *dev_name, int speed)
+{
+ struct termios t;
+ int fd;
+ int spd;
+
+ switch(speed)
+ {
+ case 115200: spd=B115200; break;
+ case 57600: spd=B57600; break;
+ case 38400: spd=B38400; break;
+ case 19200: spd=B19200; break;
+ case 9600: spd=B9600; break;
+ default: return -2;
+ }
+
+ fd = open (dev_name, O_RDWR);
+
+ if(fd<0) return -1;
+
+
+ tcgetattr (fd, &t);
+ t.c_iflag = IGNBRK | IGNPAR;
+ t.c_oflag = t.c_lflag = t.c_line = 0;
+ t.c_cflag = CS8 | CREAD | CLOCAL | HUPCL | spd;
+ tcsetattr (fd, TCSAFLUSH, &t);
+
+ serial_fd = fd;
+ return 0;
+}
+
+void serial_close()
+{
+ close(serial_fd);
+}
+
+int serial_write(char *data, int len)
+{
+// printf("WS: '");
+ int i;
+// for(i=0;i<len;i++) printf("%c", data[i]);
+// printf("'\n");
+
+ return write(serial_fd, data, len);
+}
+
+
+int serial_read(char *data, int len)
+{
+ int nbytes=0;
+ while(len)
+ {
+ if(read(serial_fd, data, 1)==1) { len--;data++; nbytes++; }
+ }
+
+ return nbytes;
+};
+
+void serial_write_byte(unsigned char b)
+{
+// printf("WB: '%c'\n", b);
+ write (serial_fd,&b,1);
+}
+
+unsigned char serial_read_byte()
+{
+ unsigned char b;
+ serial_read(&b,1);
+// fprintf(stderr,"%02x ", b);
+ return b;
+
+}
+
+int serial_data_avail()
+{
+ fd_set set;
+ struct timeval tv;
+
+ FD_ZERO(&set);
+ FD_SET(serial_fd,&set);
+
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+
+ return select(serial_fd+1, &set, NULL, NULL, &tv)>0;
+}
+
+unsigned int sys_get_clock_usec()
+{
+ struct timezone tz={0,0};
+ struct timeval tv;
+
+ gettimeofday(&tv,&tz);
+
+ return tv.tv_usec + tv.tv_sec * 1000000;
+}
+
+void sys_delay(int msecs)
+{
+ usleep(msecs*1000);
+}
+
+void serial_purge()
+{
+ while(serial_data_avail()) serial_read_byte();
+}
\ No newline at end of file
diff --git a/userspace/wrsw_hal/hal_timing.c b/userspace/wrsw_hal/hal_timing.c
index b50cd932053aa34c04db20d8d303890a886aff06..2dbdc5ba448f0d3c53e67dc40f72673d33818116 100644
--- a/userspace/wrsw_hal/hal_timing.c
+++ b/userspace/wrsw_hal/hal_timing.c
@@ -12,6 +12,8 @@
#include "rt_ipc.h"
#include "hal_exports.h"
+#include "gps_resync/gps_resync.h"
+
static int timing_mode;
#define LOCK_TIMEOUT_EXT 60000
@@ -21,7 +23,8 @@ int hal_init_timing()
{
char str[128];
timeout_t lock_tmo;
-
+ int use_utc = 0;
+
if(rts_connect() < 0)
{
TRACE(TRACE_ERROR, "Failed to establish communication with the RT subsystem.");
@@ -62,22 +65,36 @@ int hal_init_timing()
break;
}
- while(!tmo_expired(&lock_tmo))
+ while(1)
{
struct rts_pll_state pstate;
+ if(tmo_expired(&lock_tmo))
+ {
+ TRACE(TRACE_ERROR, "Can't lock the PLL. If running in the GrandMaster mode, are you sure the 1-PPS and 10 MHz reference clock signals are properly connected?");
+ return -1;
+ }
+
if(rts_get_state(&pstate) < 0)
return -1;
if(pstate.flags & RTS_DMTD_LOCKED)
- return 0;
+ break;
usleep(100000);
}
+
+ if(hal_config_get_int("timing.use_nmea", &use_utc) < 0)
+ use_utc = 0;
+
+ if(timing_mode == HAL_TIMING_MODE_GRAND_MASTER && use_utc)
+ {
+ TRACE(TRACE_INFO, "re-syncing to UTC from serial port");
+ nmea_resync_ppsgen("/dev/ttyS2");
+ }
- TRACE(TRACE_ERROR, "Can't lock the PLL. If running in the GrandMaster mode, are you sure the 1-PPS and 10 MHz reference clock signals are properly connected?");
- return -1;
+ return 0;
}
int hal_get_timing_mode()