Merge branch 'jcb-HA-implementation'

Signed-off-by: Adam Wujek <adam.wujek@cern.ch>

Merge branch 'jcb-HA-implementation'
Signed-off-by: Adam Wujek <adam.wujek@cern.ch>
e859ad5f · Adam Wujek · 499ee8c8 · 656cb314 · e859ad5f · e859ad5f
Commit e859ad5f authored Apr 10, 2019 by Adam Wujek 💬
41 changed files
--- a/Kconfig
+++ b/Kconfig
--- a/Kconfig_port_timing.in
+++ b/Kconfig_port_timing.in
--- a/binaries/rt_cpu.elf
+++ b/binaries/rt_cpu.elf
--- a/configs/wrs_release_defconfig
+++ b/configs/wrs_release_defconfig
--- a/gen_kconfig_port_timing.sh
+++ b/gen_kconfig_port_timing.sh
--- a/kernel/wr_clocksource/wr_clocksource.c
+++ b/kernel/wr_clocksource/wr_clocksource.c
@@ -24,6 +24,28 @@ static __iomem struct PPSG_WB *wrcs_ppsg;

 static int wrcs_is_registered; /* no need for atomic_t or whatever */

+/* prototypes */
+static int pps_gen_busy(void);
+static cycle_t wrcs_read(struct clocksource *cs);
+
+static struct clocksource wrcs_cs = {
+	.name = "white-rabbit",
+	.rating = 450,		/* perfect... */
+	.read = wrcs_read,
+	/* no enable/disable */
+	.mask = 0xffffffff, /* We fake a 32-bit thing */
+	.max_idle_ns = 900 * 1000 * 1000, /* well, 1s... */
+	.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+
+static int pps_gen_busy(void)
+{
+	uint32_t cr = readl(&wrcs_ppsg->CR);
+	return cr & PPSG_CR_CNT_ADJ ? 0 : 1;
+}
+
+
 /* If so requested, print statistics once per second */
 static inline void wrcs_do_stats(void)
 {
@@ -46,41 +68,6 @@ static inline void wrcs_do_stats(void)
 	ncalls++;
 }

-DEFINE_SPINLOCK(wrcs_lock);
-
-static cycle_t wrcs_read(struct clocksource *cs)
-{
-	static uint32_t offset, last, this;
-	unsigned long flags;
-
-	wrcs_do_stats();
-
-	/* FIXME: identify a time jump by monitoring the tick counter */
-
-	/*
-	 * Turn the counter into a 32-bit one (see cs->mask below).
-	 * We reset at 0x3b9aca0, so without this we should use mask = 0x1f
-	 * and mac_idle = 32 ticks = 512ns. Unaffordable.
-	 */
-	spin_lock_irqsave(&wrcs_lock, flags);
-	this = readl(&wrcs_ppsg->CNTR_NSEC);
-	if (this < last)
-		offset += WRCS_FREQUENCY;
-	last = this;
-	spin_unlock_irqrestore(&wrcs_lock, flags);
-	return offset + this;
-}
-
-
-static struct clocksource wrcs_cs = {
-	.name = "white-rabbit",
-	.rating = 450,		/* perfect... */
-	.read = wrcs_read,
-	/* no enable/disable */
-	.mask = 0xffffffff, /* We fake a 32-bit thing */
-	.max_idle_ns = 900 * 1000 * 1000, /* well, 1s... */
-	.flags = CLOCK_SOURCE_IS_CONTINUOUS,
-};

 /*
 * The timer is used to check when does WR synchronize.  When that
@@ -95,7 +82,16 @@ static void wrcs_timer_fn(unsigned long unused)
 	uint32_t ticks, tai_l, tai_h;
 	int64_t tai;

-	/* Read ppsg, all fields consistently se we can use the value */
+	if ( pps_gen_busy() ){
+		printk(KERN_DEBUG " %s: Adjusting in progress \n",__func__);
+		/* NS counter adjustment in progress. Try later */
+		goto retryLater;
+	}
+
+	wrcs_cs.rating=450; /* Perfect */
+
+	printk(KERN_DEBUG " %s: Timer function \n",__func__);
+	/* Read ppsg, all fields consistently so we can use the value */
 	do {
 		tai_l = readl(&wrcs_ppsg->CNTR_UTCLO);
 		tai_h = readl(&wrcs_ppsg->CNTR_UTCHI);
@@ -104,14 +100,62 @@ static void wrcs_timer_fn(unsigned long unused)
 	tai = (typeof(tai))tai_h << 32 | tai_l;

 	/* If we are before 2010 (date +%s --date=2010-01-01), try again */
-	if (tai < 1262300400LL) {
-		mod_timer(&wrcs_timer, jiffies + HZ);
-		return;
-	}
+	if (tai < 1262300400LL)
+		goto retryLater;

 	clocksource_register_hz(&wrcs_cs, WRCS_FREQUENCY);
 	wrcs_is_registered = 1;
 	/* And don't restart the timer */
+	return;
+
+retryLater:;
+	wrcs_cs.rating=450; /* Perfect */
+	mod_timer(&wrcs_timer, jiffies + HZ);
+	return;
+
+}
+
+DEFINE_SPINLOCK(wrcs_lock);
+
+static cycle_t wrcs_read(struct clocksource *cs)
+{
+	static uint32_t offset, last, this, adjustOffset;
+	static char cntWasNeg;
+	int32_t ticksCounter;
+	unsigned long flags;
+
+	wrcs_do_stats();
+
+	/* FIXME: identify a time jump by monitoring the tick counter */
+
+	/*
+	 * Turn the counter into a 32-bit one (see cs->mask below).
+	 * We reset at 0x3b9aca0, so without this we should use mask = 0x1f
+	 * and mac_idle = 32 ticks = 512ns. Unaffordable.
+	 */
+	spin_lock_irqsave(&wrcs_lock, flags);
+
+	ticksCounter = readl(&wrcs_ppsg->CNTR_NSEC);
+	if ( (ticksCounter & 0x8000000)!=0 ) {
+		// Negative value
+		ticksCounter|= 0xF0000000;
+		if ( ! cntWasNeg ) {
+			/* Jump from positive to negative counter value */
+			adjustOffset+=-ticksCounter;
+			adjustOffset%=WRCS_FREQUENCY;
+			cntWasNeg=1;
+		}
+	} else {
+		/* the counter is positive */
+		cntWasNeg=0;
+	}
+	this=(ticksCounter+adjustOffset)%WRCS_FREQUENCY;
+	if ( this < last) {
+		offset += WRCS_FREQUENCY;
+	}
+	last = this;
+	spin_unlock_irqrestore(&wrcs_lock, flags);
+	return offset + this;
 }

 static int wrcs_init(void)

--- a/userspace/dot-config/.gitignore
+++ b/userspace/dot-config/.gitignore
 Kconfig
+Kconfig_port_timing.in
 Kconfig_vlans.in
 dot-config
 conf

--- a/userspace/dot-config/Makefile
+++ b/userspace/dot-config/Makefile
@@ -15,7 +15,7 @@ OBJDUMP         = $(CROSS_COMPILE)objdump
 CFLAGS =  -D KBUILD_NO_NLS

 # most of this is just copying stuff in
-RFILES = Kconfig Kconfig_vlans.in dot-config wrs_release_defconfig
+RFILES = Kconfig_port_timing.in Kconfig Kconfig_vlans.in dot-config wrs_release_defconfig 
 XFILES = conf mconf nconf
 FILES = $(RFILES) $(XFILES)

@@ -36,6 +36,9 @@ dot-config: $(WRS_BASE_DIR)/../.config
 Kconfig: $(WRS_BASE_DIR)/../Kconfig
 	cp $^ $@

+Kconfig_port_timing.in: $(WRS_BASE_DIR)/../Kconfig_port_timing.in
+	cp $^ $@
+
 Kconfig_vlans.in: $(WRS_BASE_DIR)/../Kconfig_vlans.in
 	cp $^ $@


--- a/userspace/include/hal_exports.h
+++ b/userspace/include/hal_exports.h
@@ -38,6 +38,9 @@
 #define HEXP_PPSG_CMD_ADJUST_NSEC 3
 #define HEXP_PPSG_CMD_POLL 4
 #define HEXP_PPSG_CMD_SET_VALID 5
+#define HEXP_PPSG_CMD_SET_TIMING_MODE 6
+#define HEXP_PPSG_CMD_GET_TIMING_MODE 7
+#define HEXP_PPSG_CMD_GET_TIMING_MODE_STATE 8

 #define HEXP_ON 1
 #define HEXP_OFF 0
@@ -56,6 +59,11 @@
 #define HAL_TIMING_MODE_GRAND_MASTER 0
 #define HAL_TIMING_MODE_FREE_MASTER 1
 #define HAL_TIMING_MODE_BC 2
+#define HAL_TIMING_MODE_DISABLED 3
+
+#define HAL_TIMING_MODE_TMDT_UNLOCKED 0
+#define HAL_TIMING_MODE_TMDT_LOCKED 1
+#define HAL_TIMING_MODE_TMDT_HOLDHOVER 2

 typedef struct {

@@ -72,6 +80,7 @@ typedef struct {
 	uint64_t current_sec;
 	uint32_t current_nsec;

+	uint32_t timing_mode;
 } hexp_pps_params_t;

 /* Port modes (hal_port_state.mode) */
@@ -87,36 +96,11 @@ typedef struct {
 #define HEXP_PORT_TSC_FALLING 2
 */

+extern struct minipc_pd __rpcdef_lock_cmd;
+extern struct minipc_pd __rpcdef_pps_cmd;
+
 /* Prototypes of functions that call on rpc */
 extern int halexp_lock_cmd(const char *port_name, int command, int priority);
 extern int halexp_pps_cmd(int cmd, hexp_pps_params_t *params);

-/* Export structures, shared by server and client for argument matching */
-#ifdef HAL_EXPORT_STRUCTURES
-
-//int halexp_lock_cmd(const char *port_name, int command, int priority);
-struct minipc_pd __rpcdef_lock_cmd = {
-	.name = "lock_cmd",
-	.retval = MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
-	.args = {
-		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_STRING, char *),
-		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
-		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
-		 MINIPC_ARG_END,
-		 },
-};
-
-//int halexp_pps_cmd(int cmd, hexp_pps_params_t *params);
-struct minipc_pd __rpcdef_pps_cmd = {
-	.name = "pps_cmd",
-	.retval = MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
-	.args = {
-		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
-		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_STRUCT, hexp_pps_params_t),
-		 MINIPC_ARG_END,
-		 },
-};
-
-#endif /* HAL_EXPORT_STRUCTURES */
-
 #endif
--- a/userspace/include/ptpd_exports.h
+++ b/userspace/include/ptpd_exports.h
@@ -4,7 +4,8 @@
 #include <stdio.h>
 #include <stdlib.h>

-#define PTPDEXP_COMMAND_TRACKING 1
+#define PTPDEXP_COMMAND_WR_TRACKING 1
+#define PTPDEXP_COMMAND_L1SYNC_TRACKING 2

 extern int ptpdexp_cmd(int cmd, int value);


--- a/userspace/libsdb/Makefile
+++ b/userspace/libsdb/Makefile

+# We are now Kconfig-based
+-include ../../.config
+
 LINUX ?= /lib/modules/$(shell uname -r)/build

 # If we compile for the kernel, we need to include real kernel headers.
@@ -19,7 +22,10 @@ OBJDUMP         = $(CROSS_COMPILE)objdump
 # calculate endianness at compile time
 ENDIAN := $(shell ./check-endian $(CC))

-CFLAGS = -Wall -ggdb -O2
+export CFLAGS_OPTIMIZATION:= ${shell echo $(CONFIG_OPTIMIZATION)}
+
+CFLAGS += $(CFLAGS_OPTIMIZATION)
+CFLAGS += -Wall
 CFLAGS += -I../include/linux -I../include # for <sdb.h>
 CFLAGS += -ffunction-sections -fdata-sections
 CFLAGS += -Wno-pointer-sign

--- a/userspace/libwr/Makefile
+++ b/userspace/libwr/Makefile
+
+# We are now Kconfig-based
+-include ../../.config
+
 OBJS = init.o fpga_io.o util.o pps_gen.o i2c.o shw_io.o i2c_bitbang.o \
       i2c_fpga_reg.o pio.o libshw_i2c.o i2c_sfp.o fan.o i2c_io.o hwiu.o \
-       ptpd_netif.o hal_client.o \
+       ptpd_netif.o hal_client.o hal_minirpc.o\
       shmem.o rt_client.o \
       dot-config.o wrs-msg.o \
       mac.o \
@@ -21,8 +25,10 @@ STRIP           = $(CROSS_COMPILE)strip
 OBJCOPY         = $(CROSS_COMPILE)objcopy
 OBJDUMP         = $(CROSS_COMPILE)objdump

-CFLAGS = -Wall -I. -O2 -ggdb \
-	-Wstrict-prototypes \
+export CFLAGS_OPTIMIZATION:= ${shell echo $(CONFIG_OPTIMIZATION)}
+
+CFLAGS += $(CFLAGS_OPTIMIZATION)
+CFLAGS += -Wall -I. -Wstrict-prototypes \
 	-DLIBWR_INTERNAL \
 	-I./include \
 	-I../include \

--- a/userspace/libwr/dot-config.c
+++ b/userspace/libwr/dot-config.c
@@ -226,7 +226,13 @@ static int libwr_cfg_read_kconfig(struct kc **all_configs,
 	if (!f)
 		return -1;
 	while (fgets(s, sizeof(s), f)) {
-		if (sscanf(s, "source %s", name) == 1) {
+		char *ss=s;
+
+		/* Remove leading spaces */
+		while ( *ss==' ' || *ss=='\t')
+			ss++;
+
+		if (sscanf(ss, "source %s", name) == 1) {
 			/* Recursive call for sourced files */
 			ret = libwr_cfg_read_kconfig(all_configs,
 						     kconfig_dirname, name,
@@ -234,7 +240,7 @@ static int libwr_cfg_read_kconfig(struct kc **all_configs,
 			if (ret)
 				break;
 		}
-		if (sscanf(s, "config %s", name) != 1)
+		if (sscanf(ss, "config %s", name) != 1)
 			continue;
 		kc = malloc(sizeof(*kc));
 		if (!kc) {

--- a/userspace/libwr/hal_minirpc.c
+++ b/userspace/libwr/hal_minirpc.c
+#include <minipc.h>
+#include <hal_exports.h> /* for exported structs/function protos */
+
+
+/* Export structures, shared by server and client for argument matching */
+
+//int halexp_lock_cmd(const char *port_name, int command, int priority);
+struct minipc_pd __rpcdef_lock_cmd = {
+	.name = "lock_cmd",
+	.retval = MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
+	.args = {
+		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_STRING, char *),
+		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
+		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
+		 MINIPC_ARG_END,
+		 },
+};
+
+//int halexp_pps_cmd(int cmd, hexp_pps_params_t *params);
+struct minipc_pd __rpcdef_pps_cmd = {
+	.name = "pps_cmd",
+	.retval = MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
+	.args = {
+		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_INT, int),
+		 MINIPC_ARG_ENCODE(MINIPC_ATYPE_STRUCT, hexp_pps_params_t),
+		 MINIPC_ARG_END,
+		 },
+};
--- a/userspace/libwr/include/libwr/hal_shmem.h
+++ b/userspace/libwr/include/libwr/hal_shmem.h
@@ -39,6 +39,9 @@ typedef struct hal_port_calibration {
 	uint32_t delta_tx_phy;
 	uint32_t delta_rx_phy;

+	/* bit slide expresse in picos */
+	uint32_t bitslide_ps;
+
 	/* Current board routing delays (between the DDMTD inputs to
 	   the PHY clock inputs/outputs), in picoseconds */
 	uint32_t delta_tx_board;
@@ -71,9 +74,6 @@ struct hal_port_state {
 	int fd;
 	int hw_addr_auto;

-	/* port timing mode (HEXP_PORT_MODE_xxxx) */
-	int mode;
-
 	/* port FSM state (HAL_PORT_STATE_xxxx) */
 	int state;


--- a/userspace/libwr/include/libwr/pps_gen.h
+++ b/userspace/libwr/include/libwr/pps_gen.h
@@ -42,4 +42,14 @@ int shw_pps_gen_in_term_read(void);
 /* Enables PPS_IN 50Ohm termination based on dot-config option */
 int shw_pps_gen_in_term_init(void);

+/* Get the timing mode state */
+int shw_pps_get_timing_mode_state(void);
+
+/* Get the timing mode */
+int shw_pps_get_timing_mode(void);
+
+/* Set the timing mode */
+int shw_pps_set_timing_mode(int tm);
+
+
 #endif /* __LIBWR_PPS_GEN_H */
--- a/userspace/libwr/pps_gen.c
+++ b/userspace/libwr/pps_gen.c
@@ -11,6 +11,7 @@
 #include <sys/time.h>

 #include <fpga_io.h>
+#include <rt_ipc.h>
 #include <regs/ppsg-regs.h>

 #include <libwr/switch_hw.h>
@@ -95,6 +96,62 @@ int shw_pps_gen_enable_output_read(void)
 		PPSG_PPS_OUT_ENABLE : PPSG_PPS_OUT_DISABLE;
 }

+int shw_pps_set_timing_mode(int tm) {
+	int mode=-1;
+	switch (tm) {
+	case HAL_TIMING_MODE_GRAND_MASTER:
+		mode=RTS_MODE_GM_EXTERNAL;
+		break;
+	case HAL_TIMING_MODE_FREE_MASTER:
+		mode=RTS_MODE_GM_FREERUNNING;
+		break;
+	case HAL_TIMING_MODE_BC:
+		mode=RTS_MODE_BC;
+		break;
+	default :
+		pr_error("%s: Invalid timing mode %d\n", __func__, tm);
+		return -1;
+	}
+	if ( rts_set_mode(mode)==-1)  {
+		pr_error("%s: Cannot set timing mode to %d (HAL_TIMING_...)\n", __func__, tm);
+		return -1;
+	}
+	return tm;
+}
+
+int shw_pps_get_timing_mode(void) {
+	struct rts_pll_state state;
+
+	if ( rts_get_state(&state)==-1 ) {
+		return -1;
+	}
+	switch ( state.mode ) {
+	case RTS_MODE_GM_EXTERNAL :
+		return HAL_TIMING_MODE_GRAND_MASTER;
+	break;
+	case RTS_MODE_GM_FREERUNNING :
+		return RTS_MODE_GM_FREERUNNING;
+	break;
+	case RTS_MODE_BC :
+		return HAL_TIMING_MODE_BC;
+	break;
+	case RTS_MODE_DISABLED :
+		return HAL_TIMING_MODE_DISABLED;
+	default :
+		return -1;
+	}
+}
+
+int shw_pps_get_timing_mode_state(void) {
+	struct rts_pll_state state;
+
+	if ( rts_get_state(&state)==-1 ) {
+		return -1;
+	}
+	/* In the future, we should be able to provide also the state HAL_TIMING_MODE_TMDT_HOLDOVER */
+	return (state.flags & RTS_DMTD_LOCKED) ? HAL_TIMING_MODE_TMDT_LOCKED : HAL_TIMING_MODE_TMDT_UNLOCKED;
+}
+
 void shw_pps_gen_read_time(uint64_t * seconds, uint32_t * nanoseconds)
 {
 	uint32_t ns_cnt;

--- a/ppsi @ be8e0372
+++ b/ppsi @ be8e0372
-Subproject commit 2dc1b9094a287167ff6297a43e34f4d25e50acf4
+Subproject commit be8e037238a2ca33e636f26ba693a5520192b97e
--- a/userspace/rootfs_override/etc/init.d/dropbear
+++ b/userspace/rootfs_override/etc/init.d/dropbear
@@ -20,6 +20,11 @@ start_counter() {

 start() {
 	echo -n "Starting dropbear sshd: "
+ 	# copy authorized keys if exists
+	if [ -f /usr/authorized_keys ] ; then
+		mkdir -p /root/.ssh/
+		cp /usr/authorized_keys /root/.ssh/
+	fi
 	# Make sure dropbear directory exists
 	if [ ! -d /etc/dropbear ] ; then
 		mkdir -p /etc/dropbear

--- a/userspace/rootfs_override/wr/bin/assembly_ppsi_conf.sh
+++ b/userspace/rootfs_override/wr/bin/assembly_ppsi_conf.sh
--- a/userspace/rootfs_override/wr/etc/ppsi-custom.conf
+++ b/userspace/rootfs_override/wr/etc/ppsi-custom.conf
@@ -7,90 +7,90 @@ clock-accuracy 254
 port wri1
 iface wri1
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri2
 iface wri2
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri3
 iface wri3
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri4
 iface wri4
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri5
 iface wri5
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri6
 iface wri6
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri7
 iface wri7
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri8
 iface wri8
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri9
 iface wri9
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri10
 iface wri10
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri11
 iface wri11
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri12
 iface wri12
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri13
 iface wri13
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri14
 iface wri14
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri15
 iface wri15
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri16
 iface wri16
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri17
 iface wri17
 role auto
-extension whiterabbit
+profile whiterabbit

 port wri18
 iface wri18
 role auto
-extension whiterabbit
+profile whiterabbit

--- a/userspace/rootfs_override/wr/etc/ppsi-pre.conf
+++ b/userspace/rootfs_override/wr/etc/ppsi-pre.conf
 # Global settings
-
-clock-class 187
-clock-accuracy 254
-
-
+# This file is used to declare global settings which are not defined or left empty 
+# in the dot-config file.
--- a/userspace/snmpd/snmp_shmem.c
+++ b/userspace/snmpd/snmp_shmem.c
@@ -10,7 +10,8 @@ int hal_nports_local;
 /* PPSI */
 struct wrs_shm_head *ppsi_head;
 static struct pp_globals *ppg;
-struct wr_servo_state *ppsi_servo;
+struct pp_servo *ppsi_servo;
+struct wr_servo_state *ppsi_wr_servo;
 struct pp_instance *ppsi_ppi;
 int *ppsi_ppi_nlinks;

@@ -111,11 +112,13 @@ static int init_shm_ppsi(void)
 	}
 	ppg = (void *)ppsi_head + ppsi_head->data_off;

-	ppsi_servo = wrs_shm_follow(ppsi_head, ppg->global_ext_data);
-	if (!ppsi_servo) {
-		snmp_log(LOG_ERR, "Cannot follow ppsi_servo in shmem.\n");
-		return 4;
-	}
+	/* TODO JCB Servo is part of an instance now */
+	ppsi_servo=NULL;
+//	ppsi_servo = wrs_shm_follow(ppsi_head, ppg->servo);
+//	if (!ppsi_servo) {
+//		snmp_log(LOG_ERR, "Cannot follow ppsi_servo in shmem.\n");
+//		return 4;
+//	}

 	ppsi_ppi = wrs_shm_follow(ppsi_head, ppg->pp_instances);
 	if (!ppsi_ppi) {

--- a/userspace/snmpd/snmp_shmem.h
+++ b/userspace/snmpd/snmp_shmem.h
@@ -15,7 +15,7 @@ extern int hal_nports_local;

 /* PPSI */
 extern struct wrs_shm_head *ppsi_head;
-extern struct wr_servo_state *ppsi_servo;
+extern struct pp_servo *ppsi_servo;
 extern struct pp_instance *ppsi_ppi;
 extern int *ppsi_ppi_nlinks;


--- a/userspace/snmpd/wrsPortStatusTable.c
+++ b/userspace/snmpd/wrsPortStatusTable.c
@@ -93,10 +93,6 @@ time_t wrsPortStatusTable_data_fill(unsigned int *n_rows)
 			*/
 			wrsPortStatusTable_array[i].wrsPortStatusLink =
 					1 + state_up(port_state->state);
-			 /* values defined as
-			  * WRS_PORT_STATUS_CONFIGURED_MODE_* */
-			wrsPortStatusTable_array[i].wrsPortStatusConfiguredMode =
-							port_state->mode;
 			if (port_state->state == HAL_PORT_STATE_DISABLED) {
 				wrsPortStatusTable_array[i].wrsPortStatusSfpError =
 					  WRS_PORT_STATUS_SFP_ERROR_PORT_DOWN;

--- a/userspace/snmpd/wrsPtpDataTable.c
+++ b/userspace/snmpd/wrsPtpDataTable.c
@@ -83,34 +83,32 @@ time_t wrsPtpDataTable_data_fill(unsigned int *n_rows)
 	while (1) {
 		ii = wrs_shm_seqbegin(ppsi_head);

-		strncpy(wrsPtpDataTable_array[0].wrsPtpServoState,
-			ppsi_servo->servo_state_name,
-			sizeof(ppsi_servo->servo_state_name));
-		wrsPtpDataTable_array[0].wrsPtpServoStateN = ppsi_servo->state;
-		/* Keep value 0 for Not available */
-		wrsPtpDataTable_array[0].wrsPtpPhaseTracking =
-					1 + ppsi_servo->tracking_enabled;
-		wrsPtpDataTable_array[0].wrsPtpRTT = ppsi_servo->picos_mu;
-		wrsPtpDataTable_array[0].wrsPtpClockOffsetPs =
-						ppsi_servo->offset;
-		wrsPtpDataTable_array[0].wrsPtpClockOffsetPsHR =
-					int_saturate(ppsi_servo->offset);
-		wrsPtpDataTable_array[0].wrsPtpSkew =
-						int_saturate(ppsi_servo->skew);
-		wrsPtpDataTable_array[0].wrsPtpLinkLength =
-			(uint32_t)(ppsi_servo->delta_ms/1e12 * 300e6 / 1.55);
-		wrsPtpDataTable_array[0].wrsPtpServoUpdates =
-						ppsi_servo->update_count;
-		wrsPtpDataTable_array[0].wrsPtpDeltaTxM = ppsi_servo->delta_tx_m;
-		wrsPtpDataTable_array[0].wrsPtpDeltaRxM = ppsi_servo->delta_rx_m;
-		wrsPtpDataTable_array[0].wrsPtpDeltaTxS = ppsi_servo->delta_tx_s;
-		wrsPtpDataTable_array[0].wrsPtpDeltaRxS = ppsi_servo->delta_rx_s;
-		wrsPtpDataTable_array[0].wrsPtpServoStateErrCnt = ppsi_servo->n_err_state;
-		wrsPtpDataTable_array[0].wrsPtpClockOffsetErrCnt = ppsi_servo->n_err_offset;
-		wrsPtpDataTable_array[0].wrsPtpRTTErrCnt = ppsi_servo->n_err_delta_rtt;
-		wrsPtpDataTable_array[0].wrsPtpServoUpdateTime =
-			ppsi_servo->update_time.secs * 1000 * 1000 * 1000
-			+ (ppsi_servo->update_time.scaled_nsecs >> 16);
+		//TODO JCB	: Search servo instance to display
+//		strncpy(wrsPtpDataTable_array[0].wrsPtpServoState,
+//			ppsi_servo->servo_state_name,
+//			sizeof(ppsi_servo->servo_state_name));
+//		wrsPtpDataTable_array[0].wrsPtpServoStateN = ppsi_servo->state;
+//		/* Keep value 0 for Not available */
+//		wrsPtpDataTable_array[0].wrsPtpPhaseTracking = 0; //JCB TODO	1 + ppsi_servo->tracking_enabled;
+//		wrsPtpDataTable_array[0].wrsPtpRTT =  0; //JCB TODO	ppsi_servo->picos_mu;
+//		wrsPtpDataTable_array[0].wrsPtpClockOffsetPs = pp_time_to_picos(&ppsi_servo->offsetFromMaster);
+//		wrsPtpDataTable_array[0].wrsPtpClockOffsetPsHR =
+//					int_saturate(wrsPtpDataTable_array[0].wrsPtpClockOffsetPs);
+//		wrsPtpDataTable_array[0].wrsPtpSkew = 0; //JCB TODO int_saturate(ppsi_servo->skew);
+//		wrsPtpDataTable_array[0].wrsPtpLinkLength =
+//				(uint32_t)(pp_time_to_picos(&ppsi_servo->delayMS)/1e12 * 300e6 / 1.55);
+//		wrsPtpDataTable_array[0].wrsPtpServoUpdates =
+//						ppsi_servo->update_count;
+//		wrsPtpDataTable_array[0].wrsPtpDeltaTxM = 0;//JCB TODO ppsi_servo->delta_txm_ps;
+//		wrsPtpDataTable_array[0].wrsPtpDeltaRxM = 0;//JCB TODO ppsi_servo->delta_rxm_ps;
+//		wrsPtpDataTable_array[0].wrsPtpDeltaTxS = 0;//JCB TODO ppsi_servo->delta_txs_ps;
+//		wrsPtpDataTable_array[0].wrsPtpDeltaRxS = 0;//JCB TODO ppsi_servo->delta_rxs_ps;
+//		wrsPtpDataTable_array[0].wrsPtpServoStateErrCnt = 0;//JCB TODO ppsi_servo->n_err_state;
+//		wrsPtpDataTable_array[0].wrsPtpClockOffsetErrCnt = 0;//JCB TODO ppsi_servo->n_err_offset;
+//		wrsPtpDataTable_array[0].wrsPtpRTTErrCnt = 0;//JCB TODO ppsi_servo->n_err_delta_rtt;
+//		wrsPtpDataTable_array[0].wrsPtpServoUpdateTime = 0;
+//		//JCB TODO ppsi_servo->update_time.secs * 1000 * 1000 * 1000
+//		//JCB TODO + (ppsi_servo->update_time.scaled_nsecs >> 16);
 		retries++;
 		if (retries > 100) {
 			snmp_log(LOG_ERR, "%s: too many retries to read PPSI\n",

--- a/userspace/tools/Makefile
+++ b/userspace/tools/Makefile
+
+# We are now Kconfig-based
+-include ../../.config
+
 TOOLS = rtu_stat wr_mon wr_phytool wrs_pps_control spll_dbg_proxy load-lm32 load-virtex com
 TOOLS += mapper wmapper
 TOOLS += wrs_version wr_date lm32-vuart wrs_pstats
@@ -28,7 +32,10 @@ OBJCOPY         = $(CROSS_COMPILE)objcopy
 OBJDUMP         = $(CROSS_COMPILE)objdump

 # LOTs of includes
-CFLAGS =  -O2 -g -Wall \
+export CFLAGS_OPTIMIZATION:= ${shell echo $(CONFIG_OPTIMIZATION)}
+
+CFLAGS += $(CFLAGS_OPTIMIZATION)
+CFLAGS += -Wall \
 		-Wstrict-prototypes \
 		-I$(LINUX)/arch/arm/mach-at91/include \
 		-I../wrsw_rtud \
@@ -69,10 +76,10 @@ check:
 	$(CC) $*.o $(LDFLAGS) -o $*

 VPATH+=../ppsi/tools
-wrs_dump_shmem: wrs_dump_shmem.o wrs_dump_shmem_ppsi.o
+wrs_dump_shmem: wrs_dump_shmem.o wrs_dump_shmem_ppsi.o time_lib.o
 	${CC} -o $@ $^ $(LDFLAGS)

-wr_mon: wr_mon.o term.o
+wr_mon: wr_mon.o term.o  time_lib.o
 	${CC} -o $@ $^ $(LDFLAGS)

 wr_management: wr_management.o term.o

--- a/userspace/tools/time_lib.c
+++ b/userspace/tools/time_lib.c
+/**
+ * Time to string conversion functions
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <inttypes.h>
+#include <ppsi/ppsi.h>
+
+char * timeIntervalToString(TimeInterval time,char *buf) {
+
+	int64_t sign,nanos,picos;
+
+	if ( time<0 && time !=INT64_MIN) {
+		sign=-1;
+		time=-time;
+	} else {
+		sign=1;
+	}
+	nanos = time >> TIME_INTERVAL_FRACBITS;
+	picos = (((time & TIME_INTERVAL_FRACMASK) * 1000) + TIME_INTERVAL_ROUNDING_VALUE ) >> TIME_INTERVAL_FRACBITS;
+	sprintf(buf,"%" PRId64 ".%03" PRId64, sign*nanos,picos);
+	return buf;
+}
+
+char * timeToString(struct pp_time *time, char *buf) {
+	char sign = '+';
+	int64_t scaled_nsecs = time->scaled_nsecs;
+	int64_t secs = time->secs, nanos, picos;
+
+	if (!is_incorrect(time)) {
+		if (scaled_nsecs < 0 || secs < 0) {
+			sign = '-';
+			scaled_nsecs = -scaled_nsecs;
+			secs = -secs;
+		}
+		nanos = scaled_nsecs >> TIME_FRACBITS;
+		picos = ((scaled_nsecs & TIME_FRACMASK) * 1000 + TIME_ROUNDING_VALUE)
+				>> TIME_FRACBITS;
+		sprintf(buf,"%c%" PRId64 ".%09" PRId64 "%03" PRId64,
+			sign,secs,nanos,picos);
+	} else {
+		sprintf(buf, "--Incorrect--");
+	}
+	return buf;
+}
+
+char * timestampToString(struct Timestamp *time,char *buf){
+		uint64_t sec=(time->secondsField.msb << sizeof(time->secondsField.msb)) + time->secondsField.lsb;
+		sprintf(buf,"%" PRIu64 ".%09" PRIu32 "000",
+				sec, (uint32_t)time->nanosecondsField);
+		return buf;
+}
+
+char * relativeDifferenceToString(RelativeDifference time, char *buf ) {
+    int32_t nsecs=time >> REL_DIFF_FRACBITS;
+	uint64_t sub_yocto=0;
+    int64_t fraction;
+	uint64_t bitWeight=500000000000000000;
+	uint64_t mask;
+
+
+    fraction=time & REL_DIFF_FRACMASK;
+	for (mask=(uint64_t) 1<< (REL_DIFF_FRACBITS-1);mask!=0; mask>>=1 ) {
+		if ( mask & fraction )
+			sub_yocto+=bitWeight;
+		bitWeight/=2;
+	}
+	sprintf(buf,"%"PRId32".%018"PRIu64, nsecs, sub_yocto);
+	return buf;
+}
--- a/userspace/tools/time_lib.h
+++ b/userspace/tools/time_lib.h
+#include <ppsi/ppsi.h>
+
+/* Prototypes */
+char * timeIntervalToString(TimeInterval time,char *buf);
+char * timeToString(struct pp_time *time,char *buf);
+char * timestampToString(struct Timestamp *time,char *buf);
+char * relativeDifferenceToString(RelativeDifference time, char *buf );
--- a/userspace/tools/wr_date.c
+++ b/userspace/tools/wr_date.c
@@ -29,6 +29,9 @@
 /* Address for hardware, from nic-hardware.h */
 #define FPGA_BASE_PPSG  0x10010500

+extern int init_module(void *module, unsigned long len, const char *options);
+extern int delete_module(const char *module, unsigned int flags);
+
 void help(char *prgname)
 {
 	fprintf(stderr, "%s: Use: \"%s [<options>] <cmd> [<args>]\n",
@@ -43,6 +46,7 @@ void help(char *prgname)
 		"    get tohost      print WR time and set system time\n"
 		"    set <value>     set WR time to scalar seconds\n"
 		"    set host        set TAI from current host time\n"
+		"    stat            print statistics between TAI (WR time) and linux UTC\n"
 /*		"    set ntp         set TAI from ntp and leap seconds" */
 /*		"    set ntp:<ip>    set from specified ntp server\n" */
 		, WRDATE_CFG_FILE);
@@ -216,16 +220,108 @@ int fix_host_tai(void)
 	return tai_offset;
 }

+#define ADJ_SEC_ITER 10
+
+static int wait_wr_adjustment(struct PPSG_WB *pps)
+{
+	int count=0;
+
+	while ((pps->CR & PPSG_CR_CNT_ADJ)==0) {
+		if ( (count++)>=ADJ_SEC_ITER ) {
+			fprintf(stderr, "%s: warning: WR time adjustment not finished after %ds !!!\n",prgname,ADJ_SEC_ITER);
+			return 0;
+		}
+		if (opt_verbose)
+			printf("WR time adjustment: waiting.\n");
+		sleep(1);
+	}
+	if (opt_verbose)
+		printf("WR time adjustment: done.\n");
+	return 1;
+}
+
+#define CLOCK_SOURCE_MODULE_NAME "wr_clocksource"
+#define CLOCK_SOURCE_MODULE_ELF  "/wr/lib/modules/" CLOCK_SOURCE_MODULE_NAME ".ko"
+
+int removeClockSourceModule(void) {
+  int ret= delete_module(CLOCK_SOURCE_MODULE_NAME, 0);
+  if ( ret<0 ) {
+		fprintf(stderr, "%s: Warning: Cannot remove module "CLOCK_SOURCE_MODULE_NAME " : error=%s\n" ,
+				prgname,
+				strerror(errno));
+		return 0;
+  }
+  if (opt_verbose) {
+	  printf("Driver module "CLOCK_SOURCE_MODULE_NAME" removed.\n");
+  }
+  return 1;
+}
+
+int installClockSourceModule(void) {
+    struct stat st;
+    void *image=NULL;
+    int fd;
+    int ret=0;
+    const char *params="";
+
+    if ((fd = open(CLOCK_SOURCE_MODULE_ELF, O_RDONLY))<0 ) {
+		fprintf(stderr, "%s: Warning: Cannot open file "CLOCK_SOURCE_MODULE_ELF " : error=%s\n" ,
+				prgname,
+				strerror(errno));
+		goto out;;
+    }
+    if ( fstat(fd, &st)<0 ) {
+		fprintf(stderr, "%s: Warning: Cannot stat file "CLOCK_SOURCE_MODULE_ELF " : error=%s\n" ,
+				prgname,
+				strerror(errno));
+		goto out;
+    }
+    if ( (image = malloc(st.st_size)) ==NULL ) {
+		fprintf(stderr, "%s: Warning: Cannot allocate memory : error=%s\n" ,
+				prgname,
+				strerror(errno));
+		goto out;
+    }
+    if ( read(fd, image, st.st_size)< 0 ) {
+		fprintf(stderr, "%s: Warning: Cannot read file "CLOCK_SOURCE_MODULE_ELF " : error=%s\n" ,
+				prgname,
+				strerror(errno));
+		goto out;
+    }
+    if (init_module(image, st.st_size, params) != 0) {
+		fprintf(stderr, "%s: Warning: Cannot init module "CLOCK_SOURCE_MODULE_NAME " : error=%s\n" ,
+				prgname,
+				strerror(errno));
+		goto out;
+    }
+    ret=1;
+
+    out:;
+    if ( fd >=0 )
+    	close(fd);
+    if ( image!=NULL)
+    	free(image);
+    return ret;
+}
+
 /* This sets WR time from host time */
-int wrdate_internal_set(struct PPSG_WB *pps)
+int __wrdate_internal_set(struct PPSG_WB *pps, int deep)
+
 {
 	struct timeval tvh, tvr; /* host, rabbit */
 	signed long long diff64;
 	signed long diff;
 	int tai_offset;
+	int modRemoved=0;

+	if ( deep > 4 )
+		return 0; /* Avoid stack overflow (recursive function) in case of error */
+	if ( deep==0 ) {
+		modRemoved=removeClockSourceModule(); // The driver must be removed otherwise the time cannot be set properly
+	}
 	tai_offset = fix_host_tai();

+	usleep(100);
 	gettimeofday(&tvh, NULL);
 	gettimeof_wr(&tvr, pps);

@@ -246,7 +342,7 @@ int wrdate_internal_set(struct PPSG_WB *pps)
 		fprintf(stderr, "%s: Warning: fractional second differs by"
 			"more than 0.2 (%li ms)\n", prgname, diff / 1000);

-	if (opt_verbose) {
+	if (opt_verbose && deep==0) {
 		printf("Host time: %9li.%06li\n", (long)(tvh.tv_sec),
 		       (long)(tvh.tv_usec));
 		printf("WR   time: %9li.%06li\n", (long)(tvr.tv_sec),
@@ -263,18 +359,41 @@ int wrdate_internal_set(struct PPSG_WB *pps)
 		pps->ADJ_NSEC = 0;
 		asm("" : : : "memory"); /* barrier... */
 		pps->CR = pps->CR | PPSG_CR_CNT_ADJ;
+		if ( wait_wr_adjustment(pps) )
+		__wrdate_internal_set(pps,deep+1); /* adjust the usecs */
 	} else {
 		if (opt_verbose)
 			printf("adjusting by %li usecs\n", diff);
 		pps->ADJ_UTCLO = 0;
 		pps->ADJ_UTCHI = 0;
-		pps->ADJ_NSEC = diff * 64 + diff / 2;
+		pps->ADJ_NSEC = (diff*1000)/16;
 		asm("" : : : "memory"); /* barrier... */
 		pps->CR = pps->CR | PPSG_CR_CNT_ADJ;
+		wait_wr_adjustment(pps);
+	}
+
+	if ( deep==0 && modRemoved ) {
+		installClockSourceModule();
+	}
+
+	if (opt_verbose && deep==0) {
+		usleep(100);
+		gettimeofday(&tvh, NULL);
+		gettimeof_wr(&tvr, pps);
+
+		printf("Host time: %9li.%06li\n", (long)(tvh.tv_sec),
+		       (long)(tvh.tv_usec));
+		printf("WR   time: %9li.%06li\n", (long)(tvr.tv_sec),
+		       (long)(tvr.tv_usec));
 	}
 	return 0;
 }

+/* This sets WR time from host time */
+int wrdate_internal_set(struct PPSG_WB *pps) {
+	return __wrdate_internal_set(pps,0);
+}
+
 /* Frontend to the set mechanism: parse the argument */
 int wrdate_set(struct PPSG_WB *pps, char *arg)
 {
@@ -303,6 +422,57 @@ int wrdate_set(struct PPSG_WB *pps, char *arg)
 	return 0;
 }

+/* Print statistics between TAI and UTC dates */
+#define STAT_SAMPLE_COUNT 20
+
+int wrdate_stat(struct PPSG_WB *pps)
+{
+	int udiff_ref=0,udiff_last;
+
+	printf("Diff_TAI_UTC[sec] Diff_with_last[usec] Diff_with_ref[usec]\n");
+	while ( 1 ) {
+		int64_t udiff_arr[STAT_SAMPLE_COUNT]; // Diff in useconds
+		struct timeval tv_tai,tv_host;
+		int i;
+		int64_t udiff_sum=0, udiff;
+
+		for ( i=0; i<STAT_SAMPLE_COUNT; i++ ) {
+			int64_t *udiff_tmp=&udiff_arr[i];
+
+			// Get time
+			usleep(100); // Increase stability of measures : less preempted during time measures
+			gettimeof_wr(&tv_tai,pps);
+			gettimeofday(&tv_host, NULL);
+
+			// Calculate difference
+			*udiff_tmp=(tv_host.tv_sec-tv_tai.tv_sec)*1000000;
+			if ( tv_host.tv_usec > tv_tai.tv_usec ) {
+				*udiff_tmp+=tv_host.tv_usec-tv_tai.tv_usec;
+			} else {
+				*udiff_tmp-=tv_tai.tv_usec-tv_host.tv_usec;
+			}
+			udiff_sum+=*udiff_tmp;
+		}
+		udiff=udiff_sum/STAT_SAMPLE_COUNT;
+		if ( udiff_ref==0) {
+			udiff_ref=udiff_last=udiff;
+		}
+
+		printf("%03d.%06d %6li %6li\n",
+			   (int)(udiff/1000000),
+			   abs(udiff%1000000),
+			   (long) (udiff_last-udiff),
+			   (long) (udiff_ref-udiff)
+			   );
+		udiff_last=udiff;
+
+		sleep(1);
+	}
+
+	return 0;
+}
+
+
 int main(int argc, char **argv)
 {
 	int c, tohost = 0;
@@ -352,9 +522,15 @@ int main(int argc, char **argv)
 		return wrdate_get(pps, tohost);
 	}

+	if (!strcmp(cmd, "stat")) {
+		/* parse the optional "tohost" argument */
+		return wrdate_stat(pps);
+	}
+
 	/* only other command is "set", with one argument */
 	if (strcmp(cmd, "set") || optind != argc - 1)
 		help(argv[0]);

+
 	return wrdate_set(pps, argv[optind]);
 }
--- a/userspace/tools/wr_mon.c
+++ b/userspace/tools/wr_mon.c
--- a/userspace/tools/wr_phytool.c
+++ b/userspace/tools/wr_phytool.c
@@ -452,6 +452,27 @@ void pps_adjustment_test(int ep, int argc, char *argv[])
 	}
 }

+char * getRtsTimingMode(int mode) {
+	static struct {
+		char *name;
+		int mode;
+	} *m, modes[] = {
+		{"TIME_GM", RTS_MODE_GM_EXTERNAL},
+		{"TIME_FM", RTS_MODE_GM_FREERUNNING},
+		{"TIME_BC", RTS_MODE_BC},
+		{"TIME_DS", RTS_MODE_DISABLED},
+		{"???????",-1}
+	};
+
+	m=modes;
+	do {
+		if ( m->mode==mode )
+			break;
+		m++;
+	} while (m->mode!=-1);
+	return m->name;
+}
+
 void rt_command(int ep, int argc, char *argv[])
 {
 /* ep is 0..17 */
@@ -473,7 +494,7 @@ void rt_command(int ep, int argc, char *argv[])
 	{
 		printf("RTS State Dump [%d physical ports]:\n",
 		       hal_nports_local);
-		printf("CurrentRef: %d Mode: %d Flags: %x\n", pstate.current_ref, pstate.mode, pstate.flags);
+		printf("CurrentRef: %d Mode: %s (%d) Flags: %x\n", pstate.current_ref, getRtsTimingMode(pstate.mode), pstate.mode, pstate.flags);
 		for (i = 0; i < hal_nports_local; i++)
 			printf("wri%-2d: setpoint: %-8dps current: %-8dps "
 			       "loopback: %-8dps flags: %x\n", i + 1,
@@ -483,28 +504,27 @@ void rt_command(int ep, int argc, char *argv[])
 			       pstate.channels[i].flags);
 	} else if (!strcmp(argv[3], "lock"))
 	{
-		int i;
 		printf("locking to: port %d wri%d\n", ep + 1, ep + 1);
-		for(i=0;i<100;i++)
-		{
-			rts_get_state(&pstate);
-			printf("setmode rv %d\n", rts_set_mode(RTS_MODE_BC));
-			printf("lock rv %d\n", rts_lock_channel(ep, 0));
-		}
-	} else if (!strcmp(argv[3], "master"))
+		rts_get_state(&pstate);
+		printf("setmode rv %d\n", rts_set_mode(RTS_MODE_BC));
+		printf("lock rv %d\n", rts_lock_channel(ep, 0));
+	} else if (!strcmp(argv[3], "fr"))
 	{
-		int i;
-		printf("Enabling free-running master mode\n");
-		for(i=0;i<10;i++)
-		{
-				printf("rv: %d\n", rts_set_mode(RTS_MODE_GM_FREERUNNING));
-		}
-//		rts_lock_channel(ep);
+		printf("Enabling free-running master timing mode\n");
+		printf("rv: %d\n", rts_set_mode(RTS_MODE_GM_FREERUNNING));
+	}
+	else if (!strcmp(argv[3], "gm"))
+	{
+		printf("Enabling grand master timing mode\n");
+		printf("rv: %d\n", rts_set_mode(RTS_MODE_GM_EXTERNAL));
 	}
-	else if (!strcmp(argv[3], "track"))
+	else if (!strcmp(argv[3], "ds"))
+	{
+		printf("Disable timing mode\n");
+		printf("rv: %d\n", rts_set_mode(RTS_MODE_DISABLED));
+	}else if (!strcmp(argv[3], "track"))
 	{
 		printf("Enabling ptracker @ port %d (wri%d)\n", ep + 1, ep + 1);
-
 		rts_enable_ptracker(ep, 1);
 	}
 }
@@ -559,7 +579,7 @@ struct {
 	{
 	"rt",
 	"",
-	"RT subsystem command [show,lock,master,gm]",
+	"RT subsystem command [show,lock,[gm,fr,ds],track]",
 	rt_command},
 	{NULL}


--- a/userspace/tools/wrs_dump_shmem.c
+++ b/userspace/tools/wrs_dump_shmem.c
@@ -8,6 +8,7 @@
 #include <fcntl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
+#include <inttypes.h>
 #include <sys/mman.h>
 #include <libwr/shmem.h>
 #include <libwr/hal_shmem.h>
@@ -16,6 +17,13 @@
 #include <libwr/util.h>
 #include <ppsi/ppsi.h>
 #include <ppsi-wrs.h>
+#include "time_lib.h"
+
+/*  be safe, in case some other header had them slightly differently */
+#undef container_of
+#undef offsetof
+#undef ARRAY_SIZE
+
 #include "wrs_dump_shmem.h"

 #define FPGA_SPLL_STAT 0x10006800
@@ -72,17 +80,44 @@ static int dump_all_rtu_entries = 0; /* rtu exports 4096 vlans and 2048 htab
 				 entries */


-void dump_one_field(void *addr, struct dump_info *info)
+#define REL_DIFF_FRACBITS 62
+#define REL_DIFF_FRACMASK 0x3fffffffffffffff
+
+void decode_relative_difference(RelativeDifference rd, int32_t *nsecs, uint64_t *sub_yocto) {
+    int64_t fraction;
+	uint64_t bitWeight=500000000000000000;
+	uint64_t mask;
+
+	*sub_yocto=0;
+	*nsecs = (int32_t)(rd >> REL_DIFF_FRACBITS);
+    fraction=(int64_t)rd & REL_DIFF_FRACMASK;
+	for (mask=(uint64_t) 1<< (REL_DIFF_FRACBITS-1);mask!=0; mask>>=1 ) {
+		if ( mask & fraction )
+			*sub_yocto+=bitWeight;
+		bitWeight/=2;
+	}
+}
+
+void dump_one_field(void *addr, struct dump_info *info, char *info_prefix)
 {
 	void *p = addr + info->offset;
+	char buf[128];
 	struct pp_time *t = p;
+	RelativeDifference *rd=p;
+	Timestamp *ts=p;
+	TimeInterval *ti=p;
 	struct PortIdentity *pi = p;
 	struct ClockQuality *cq = p;
 	char format[16];
-	long nano, pico;
 	int i;
+	char pname[128];
+
+	if (info_prefix!=NULL )
+		sprintf(pname,"%s.%s",info_prefix,info->name);
+	else
+		strcpy(pname,info->name);

-	printf("        %-30s ", info->name); /* name includes trailing ':' */
+	printf("%-40s ", pname); /* name includes trailing ':' */
 	switch(info->type) {
 	case dump_type_char:
 		sprintf(format,"\"%%.%is\"\n", info->size);
@@ -108,6 +143,7 @@ void dump_one_field(void *addr, struct dump_info *info)
 	case dump_type_uint64_t:
 		printf("%lld\n", *(unsigned long long *)p);
 		break;
+	case dump_type_long_long:
 	case dump_type_Integer64:
 		printf("%lld\n", *(long long *)p);
 		break;
@@ -129,6 +165,9 @@ void dump_one_field(void *addr, struct dump_info *info)
 	case dump_type_Boolean:
 		printf("%i\n", *(unsigned char *)p);
 		break;
+	case dump_type_UInteger4:
+		printf("%i\n", *(unsigned char *)p & 0xF);
+		break;
 	case dump_type_UInteger16:
 	case dump_type_uint16_t:
 	case dump_type_unsigned_short:
@@ -146,14 +185,22 @@ void dump_one_field(void *addr, struct dump_info *info)
 	case dump_type_Integer16:
 		printf("%i\n", *(short *)p);
 		break;
+
 	case dump_type_time:
-		nano = t->scaled_nsecs >> 16;
-		pico = t->scaled_nsecs & 0xffff;
-		pico = (pico * 1000) >> 16;
-		printf("correct %i: %10lli.%09li.%03li\n",
-		       !is_incorrect(t), t->secs, nano,pico);
+		printf("%s\n",timeToString(t,buf));
 		break;

+	case dump_type_Timestamp:
+		printf("%s\n",timestampToString(ts,buf));
+		break;
+
+	case dump_type_TimeInterval:
+		printf("%s\n",timeIntervalToString(*ti,buf));
+		break;
+
+	case dump_type_RelativeDifference:
+		printf("%s\n",relativeDifferenceToString(*rd,buf));
+		break;
 	case dump_type_ip_address:
 		for (i = 0; i < 4; i++)
 			printf("%02x%c", ((unsigned char *)p)[i],
@@ -174,7 +221,7 @@ void dump_one_field(void *addr, struct dump_info *info)
 		break;

 	case dump_type_ClockQuality:
-		printf("class %i, accuracy %02x (%i), logvariance %i\n",
+		printf("class=%i, accuracy=0x%02x (%i), logvariance=%i\n",
 		       cq->clockClass, cq->clockAccuracy, cq->clockAccuracy,
 		       cq->offsetScaledLogVariance);
 		break;
@@ -305,7 +352,8 @@ void dump_one_field(void *addr, struct dump_info *info)
 		break;
 	}
 }
-void dump_many_fields(void *addr, struct dump_info *info, int ninfo)
+
+void dump_many_fields(void *addr, struct dump_info *info, int ninfo, char *prefix)
 {
 	int i;

@@ -313,8 +361,9 @@ void dump_many_fields(void *addr, struct dump_info *info, int ninfo)
 		fprintf(stderr, "dump: pointer not valid\n");
 		return;
 	}
-	for (i = 0; i < ninfo; i++)
-		dump_one_field(addr, info + i);
+	for (i = 0; i < ninfo; i++) {
+		dump_one_field(addr, info + i,prefix);
+	}
 }

 /* the macro below relies on an externally-defined structure type */
@@ -352,7 +401,6 @@ struct dump_info hal_port_info [] = {
 	DUMP_FIELD(int, hw_index),
 	DUMP_FIELD(int, fd),
 	DUMP_FIELD(int, hw_addr_auto),
-	DUMP_FIELD(port_mode, mode),
 	DUMP_FIELD(int, state),
 	DUMP_FIELD(int, fiber_index),
 	DUMP_FIELD(int, locked),
@@ -411,7 +459,7 @@ int dump_hal_mem(struct wrs_shm_head *head)
 	h = (void *)head + head->data_off;

 	/* dump hal's shmem */
-	dump_many_fields(h, hal_shmem_info, ARRAY_SIZE(hal_shmem_info));
+	dump_many_fields(h, hal_shmem_info, ARRAY_SIZE(hal_shmem_info),"HAL");

 	n = h->nports;
 	p = wrs_shm_follow(head, h->ports);
@@ -422,8 +470,10 @@ int dump_hal_mem(struct wrs_shm_head *head)
 	}

 	for (i = 0; i < n; i++, p++) {
-		printf("dump port %i\n", i + 1);
-		dump_many_fields(p, hal_port_info, ARRAY_SIZE(hal_port_info));
+		char prefix[64];
+
+		sprintf(prefix,"HAL.port.%d",i+1);
+		dump_many_fields(p, hal_port_info, ARRAY_SIZE(hal_port_info),prefix);
 	}
 	return 0;
 }
@@ -474,6 +524,7 @@ int dump_rtu_mem(struct wrs_shm_head *head)
 	struct rtu_filtering_entry *rtu_filters_cur;
 	struct rtu_vlan_table_entry *rtu_vlans;
 	int i, j;
+	char prefix[64];

 	if (head->version != RTU_SHMEM_VERSION) {
 		fprintf(stderr, "dump rtu: unknown version %i (known is %i)\n",
@@ -496,9 +547,9 @@ int dump_rtu_mem(struct wrs_shm_head *head)
 			    && (!rtu_filters_cur->valid))
 				/* don't display empty entries */
 				continue;
-			printf("dump htab[%d][%d]\n", i, j);
+			sprintf(prefix,"rtu.htab.%d.%d",i,j);
 			dump_many_fields(rtu_filters_cur, htab_info,
-					 ARRAY_SIZE(htab_info));
+					 ARRAY_SIZE(htab_info),prefix);
 		}
 	}

@@ -507,8 +558,8 @@ int dump_rtu_mem(struct wrs_shm_head *head)
 			    && rtu_vlans->port_mask == 0x0))
 			/* don't display empty entries */
 			continue;
-		printf("dump vlan %i\n", i);
-		dump_many_fields(rtu_vlans, vlan_info, ARRAY_SIZE(vlan_info));
+		sprintf(prefix,"rtu.vlan.%d",i);
+		dump_many_fields(rtu_vlans, vlan_info, ARRAY_SIZE(vlan_info),prefix);
 	}
 	return 0;
 }
@@ -537,8 +588,6 @@ struct dump_info spll_stats_info[] = {

 static int dump_spll_mem(struct spll_stats *spll)
 {
-	printf("ID: Soft PLL:\n");
-
 	/* Check magic */
 	if (spll->magic != SPLL_MAGIC) {
 		/* Wrong magic */
@@ -546,7 +595,7 @@ static int dump_spll_mem(struct spll_stats *spll)
 			spll->magic, SPLL_MAGIC);
 	}

-	dump_many_fields(spll, spll_stats_info, ARRAY_SIZE(spll_stats_info));
+	dump_many_fields(spll, spll_stats_info, ARRAY_SIZE(spll_stats_info),"SoftPll");

 	return 0; /* this is complete */
 }
@@ -656,20 +705,17 @@ int main(int argc, char **argv)
 		}
 		head = m;
 		if (!head->pidsequence) {
-			printf("ID %i (\"%s\"): no data\n",
-			       i, name_id_to_name[i]);
+			printf("shm.%d.name:       %s\n",i,name_id_to_name[i]);
+			printf("shm.%d.iterations: %d  (no data)\n",i,head->pidsequence);
 			wrs_shm_put(m);
 			continue;
 		}
+		printf("shm.%d.name:       %s\n",i,head->name);
+		printf("shm.%d.pid:        %d\n",i,head->pid);
 		if (head->pid) {
-			printf("ID %i (\"%s\"): pid %i (%s, %i iterations)\n",
-			       i, head->name, head->pid,
-			       kill(head->pid, 0) < 0 ? "dead" : "alive",
-			       head->pidsequence);
-		} else {
-			printf("ID %i (\"%s\"): no pid (after %i iterations)\n",
-			       i, head->name, head->pidsequence);
+			printf("shm.%d.status:     %s\n",i,kill(head->pid, 0) < 0 ? "dead" : "alive");
 		}
+		printf("shm.%d.iterations: %d\n",i,head->pidsequence);
 		f = name_id_to_f[i];

 		/* if the area-specific function fails, fall back to generic */

--- a/userspace/tools/wrs_dump_shmem.h
+++ b/userspace/tools/wrs_dump_shmem.h
@@ -38,6 +38,7 @@ enum dump_type {
 	dump_type_uint16_t,
 	dump_type_int,
 	dump_type_unsigned_long,
+	dump_type_long_long,
 	dump_type_unsigned_char,
 	dump_type_unsigned_short,
 	dump_type_double,
@@ -52,11 +53,15 @@ enum dump_type {
 	dump_type_Integer16,
 	dump_type_UInteger8,
 	dump_type_Integer8,
+	dump_type_UInteger4,
 	dump_type_Enumeration8,
 	dump_type_Boolean,
 	dump_type_ClockIdentity,
 	dump_type_PortIdentity,
 	dump_type_ClockQuality,
+	dump_type_TimeInterval,
+	dump_type_RelativeDifference,
+	dump_type_Timestamp,
 	/* and this is ours */
 	dump_type_time,
 	dump_type_ip_address,
@@ -88,5 +93,6 @@ struct dump_info {
 	int size;  /* only for strings or binary strings */
 };

-void dump_many_fields(void *addr, struct dump_info *info, int ninfo);
+void dump_many_fields(void *addr, struct dump_info *info, int ninfo,
+		      char *prefix);
 int dump_ppsi_mem(struct wrs_shm_head *head);
--- a/userspace/wrs_watchdog/Makefile
+++ b/userspace/wrs_watchdog/Makefile

+# We are now Kconfig-based
+-include ../../.config
+
 OBJS = wrs_watchdog.o

 BINARY = wrs_watchdog
@@ -16,7 +19,10 @@ STRIP           = $(CROSS_COMPILE)strip
 OBJCOPY         = $(CROSS_COMPILE)objcopy
 OBJDUMP         = $(CROSS_COMPILE)objdump

-CFLAGS = -O -g -Wall \
+export CFLAGS_OPTIMIZATION:= ${shell echo $(CONFIG_OPTIMIZATION)}
+
+CFLAGS += $(CFLAGS_OPTIMIZATION)
+CFLAGS = -Wall \
 	-Wstrict-prototypes \
 	-I../include \
 	-I../libwr/include \

--- a/userspace/wrsw_hal/Makefile
+++ b/userspace/wrsw_hal/Makefile

+# We are now Kconfig-based
+-include ../../.config
+
 OBJS = hal_exports.o hal_main.o hal_ports.o hal_timing.o

 BINARY = wrsw_hal
@@ -16,7 +19,10 @@ STRIP           = $(CROSS_COMPILE)strip
 OBJCOPY         = $(CROSS_COMPILE)objcopy
 OBJDUMP         = $(CROSS_COMPILE)objdump

-CFLAGS = -O -g -Wall \
+export CFLAGS_OPTIMIZATION:= ${shell echo $(CONFIG_OPTIMIZATION)}
+
+CFLAGS += $(CFLAGS_OPTIMIZATION)
+CFLAGS += -Wall \
 	-Wstrict-prototypes \
 	-I../include \
 	-I../libwr/include \
@@ -34,6 +40,7 @@ LDFLAGS = -L../libwr -L../mini-rpc \
 all: $(BINARY)

 $(BINARY): $(OBJS)
+	pwd
 	$(CC) -o $@ $^ $(LDFLAGS)

 install: all

--- a/userspace/wrsw_hal/hal_exports.c
+++ b/userspace/wrsw_hal/hal_exports.c
@@ -15,7 +15,6 @@
 #include <minipc.h>
 #include <libwr/shmem.h>

-#define HAL_EXPORT_STRUCTURES
 #include <hal_exports.h> /* for exported structs/function protos */

 static struct minipc_ch *hal_ch;
@@ -28,7 +27,7 @@ int halexp_lock_cmd(const char *port_name, int command, int priority)
 {
 	int rval;

-	pr_debug("halexp_lock_cmd: cmd=%d port=%s\n", command, port_name);
+/*	pr_debug("halexp_lock_cmd: cmd=%d port=%s\n", command, port_name); */

 	switch (command) {
 	case HEXP_LOCK_CMD_ENABLE_TRACKING:
@@ -78,8 +77,9 @@ int halexp_lock_cmd(const char *port_name, int command, int priority)
 * both the PLLs and the PPS Generator. */
 int halexp_pps_cmd(int cmd, hexp_pps_params_t * params)
 {
-	int busy;
+	int busy,ret;

+	pr_debug("halexp_pps_cmd: cmd=%d\n", cmd);
 	switch (cmd) {
 		/* fixme: TODO: implement HEXP_PPSG_CMD_GET call */

@@ -142,6 +142,19 @@ int halexp_pps_cmd(int cmd, hexp_pps_params_t * params)
 	case HEXP_PPSG_CMD_SET_VALID:
 		return shw_pps_gen_enable_output(params->pps_valid);

+	case HEXP_PPSG_CMD_SET_TIMING_MODE:
+		ret=shw_pps_set_timing_mode(params->timing_mode);
+		hal_update_timing_mode();
+		return ret;
+
+	case HEXP_PPSG_CMD_GET_TIMING_MODE:{
+		ret=hal_get_timing_mode();
+		printf("JCB: shw_pps_get_timing_mode() returns %d\n",ret);
+		return ret;
+	}
+
+	case HEXP_PPSG_CMD_GET_TIMING_MODE_STATE:
+		return shw_pps_get_timing_mode_state();
 	}
 	return -1;		/* fixme: real error code */
 }

--- a/userspace/wrsw_hal/hal_ports.c
+++ b/userspace/wrsw_hal/hal_ports.c
--- a/userspace/wrsw_hal/hal_timing.c
+++ b/userspace/wrsw_hal/hal_timing.c
@@ -15,107 +15,44 @@
 #include <rt_ipc.h>
 #include <hal_exports.h>

-static int timing_mode;
-
-#define LOCK_TIMEOUT_EXT 60000
-#define LOCK_TIMEOUT_INT 10000
+extern struct rts_pll_state hal_port_rts_state;
+extern int hal_port_rts_state_valid;

 int hal_init_timing_mode(void)
 {
-	static struct {
-		char *cfgname;
-		int modevalue;
-	} *m, modes[] = {
-		{"TIME_GM", HAL_TIMING_MODE_GRAND_MASTER},
-		{"TIME_FM", HAL_TIMING_MODE_FREE_MASTER},
-		{"TIME_BC", HAL_TIMING_MODE_BC},
-		{NULL, HAL_TIMING_MODE_BC /* default */},
-	};
-
 	if (rts_connect(NULL) < 0) {
 		pr_error(
 		      "Failed to establish communication with the RT subsystem.\n");
 		return -1;
 	}
-
-	/* Read the mode from dot-config */
-	for (m = modes; m->cfgname; m++)
-		if (libwr_cfg_get(m->cfgname))
-			break;
-	timing_mode = m->modevalue;
-
-	if (!m->cfgname)
-		pr_error("%s: no config variable set, defaults used\n",
-			__func__);
 	return 0;
 }

 int hal_init_timing(char *filename)
 {
-	timeout_t lock_tmo;
-	/* initialize the RT Subsys */
-	switch (timing_mode) {
-	case HAL_TIMING_MODE_GRAND_MASTER:
-		rts_set_mode(RTS_MODE_GM_EXTERNAL);
-		libwr_tmo_init(&lock_tmo, LOCK_TIMEOUT_EXT, 0);
-		break;
-
-	default: /* never hit, but having it here prevents a warning */
-		pr_error("%s: Unable to determine HAL mode! Use BC as"
-			 " default\n", __func__);
-	case HAL_TIMING_MODE_FREE_MASTER:
-	case HAL_TIMING_MODE_BC:
-		rts_set_mode(RTS_MODE_GM_FREERUNNING);
-		libwr_tmo_init(&lock_tmo, LOCK_TIMEOUT_INT, 0);
-		break;
-	}
-
-	while (1) {
-		struct rts_pll_state pstate;
-
-		if (libwr_tmo_expired(&lock_tmo)) {
-			pr_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?,"
-			      " retrying...\n");
-			if (timing_mode == HAL_TIMING_MODE_GRAND_MASTER) {
-				/*ups... something went wrong, try again */
-				rts_set_mode(RTS_MODE_GM_EXTERNAL);
-				libwr_tmo_init(&lock_tmo, LOCK_TIMEOUT_EXT, 0);
-			} else {
-				pr_error("Got timeout\n");
-				return -1;
-			}
-		}
-
-		if (rts_get_state(&pstate) < 0) {
-			/* Don't give up when rts_get_state fails, it may be
-			 * due to race with ppsi at boot. No problems seen
-			 * because of waiting here. */
-			pr_error("rts_get_state failed try again\n");
-			continue;
-		}
-
-		if (pstate.flags & RTS_DMTD_LOCKED) {
-			if (timing_mode == HAL_TIMING_MODE_GRAND_MASTER)
-				pr_info("GrandMaster locked to external "
-						"reference\n");
-			break;
-		}
-
-		usleep(100000);
-	}
-
-	/*
-	 * We had "timing.use_nmea", but it was hardwired to /dev/ttyS2
-	 * which is not wired out any more, so this is removed after v4.1
-	 */
-
 	return 0;
 }

-int hal_get_timing_mode()
+int hal_get_timing_mode(void)
 {
-	return timing_mode;
+	struct rts_pll_state *hs = &hal_port_rts_state;
+
+	if (hal_port_rts_state_valid)
+		switch (hs->mode) {
+		case RTS_MODE_GM_EXTERNAL:
+			return HAL_TIMING_MODE_GRAND_MASTER;
+		case RTS_MODE_GM_FREERUNNING:
+			return HAL_TIMING_MODE_FREE_MASTER;
+		case RTS_MODE_BC:
+			return HAL_TIMING_MODE_BC;
+		case RTS_MODE_DISABLED:
+			return HAL_TIMING_MODE_DISABLED;
+		}
+	return -1;
+}
+
+int  hal_update_timing_mode(void) {
+	return hal_port_poll_rts_state();
 }
+
+
--- a/userspace/wrsw_hal/wrsw_hal.h
+++ b/userspace/wrsw_hal/wrsw_hal.h
@@ -31,10 +31,12 @@ int hal_port_start_lock(const char *port_name, int priority);
 int hal_port_check_lock(const char *port_name);
 int hal_port_reset(const char *port_name);
 int hal_port_enable_tracking(const char *port_name);
+int hal_port_poll_rts_state(void);

 int hal_init_timing_mode(void);
 int hal_init_timing(char *filename);
 int hal_get_timing_mode(void);
+int hal_update_timing_mode(void);
 int hal_port_pshifter_busy(void);

 #endif
--- a/userspace/wrsw_rtud/Makefile
+++ b/userspace/wrsw_rtud/Makefile

+# We are now Kconfig-based
+-include ../../.config
+
 PROGRAM		= wrsw_rtud
 SRCFILES	= rtu_drv.c rtu_ext_drv.c rtu_hash.c rtu_fd.c rtud.c \
 			rtud_exports.c utils.c
@@ -17,7 +20,10 @@ STRIP           = $(CROSS_COMPILE)strip
 OBJCOPY         = $(CROSS_COMPILE)objcopy
 OBJDUMP         = $(CROSS_COMPILE)objdump

-CFLAGS = -O2 -Wall -ggdb  \
+export CFLAGS_OPTIMIZATION:= ${shell echo $(CONFIG_OPTIMIZATION)}
+
+CFLAGS += $(CFLAGS_OPTIMIZATION)
+CFLAGS = -Wall  \
 	-Wstrict-prototypes \
 	-I../mini-rpc \
 	-I../include \