From f3e7feaccddd5bc249be06dadd7cc96d10dcee22 Mon Sep 17 00:00:00 2001
From: Serhii Voloshynov <serhii.voloshynov@globallogic.com>
Date: Thu, 3 Dec 2020 12:59:37 +0200
Subject: [PATCH] ubx time leap seconds

---
 driver_ubx.c | 128 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 127 insertions(+), 1 deletion(-)

diff --git a/driver_ubx.c b/driver_ubx.c
index 6464d94..a5f3737 100644
--- a/driver_ubx.c
+++ b/driver_ubx.c
@@ -61,6 +61,29 @@
 #define UBX_CFG_LEN		20
 #define outProtoMask		14

+#ifdef UBLOXTIMELS_ENABLE
+/* UBX-NAV-TIMELS support */
+static char *srcOfCurrLs[] = {
+  "firmware",
+  "GPS GLONASS difference",
+  "GPS",
+  "SBAS",
+  "BeiDou",
+  "Galileo",
+  "Aided data",
+  "Configured"
+};
+static char *srcOfLsChange[] = {
+  "No Source",
+  "Undefined",
+  "GPS",
+  "SBAS",
+  "BeiDou",
+  "Galileo",
+  "GLONOSS",
+};
+#endif  /* UBLOXTIMELS_ENABLE */
+
 static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
 			    size_t len);
 static gps_mask_t ubx_msg_nav_eoe(struct gps_device_t *session,
@@ -810,6 +833,90 @@ static void ubx_msg_nav_timels(struct gps_device_t *session,
     }
 }

+#ifdef UBLOXTIMELS_ENABLE
+/**
+ * Navigation time to leap second
+ *
+ * Sets leap_notify if leap second is < 23 hours away.
+ */
+static void
+ubx_msg_nav_timels(struct gps_device_t *session, unsigned char *buf,
+		size_t data_len)
+{
+    int version;
+    unsigned int flags;
+    int valid_curr_ls;
+    int valid_time_to_ls_event;
+
+    if (data_len != 24) {
+        gpsd_log(&session->context->errout, LOG_WARN,
+	  "UBX-NAV-TIMELS: unexpected length %d, expecting 24\n",
+	  data_len);
+	return;
+    }
+    version = getsb(buf,4);
+    /* Only version 0 is defined so far. */
+    flags = (unsigned int)getub(buf, 23);
+    gpsd_log(&session->context->errout, LOG_DATA,
+        "UBX-NAV-TIMELS: flags 0x%x message version %d\n",flags, version);
+    valid_curr_ls = flags & UBX_TIMELS_VALID_CURR_LS;
+    valid_time_to_ls_event = flags & UBX_TIMELS_VALID_TIME_LS_EVT;
+    if(valid_curr_ls) {
+      unsigned int src_of_curr_ls = getub(buf,8);
+      int curr_ls = getsb(buf,9);
+      char *src = "Unknown";
+
+      if(src_of_curr_ls < (sizeof srcOfCurrLs/(sizeof srcOfCurrLs[0])))
+	src = srcOfCurrLs[src_of_curr_ls];
+
+      gpsd_log(&session->context->errout, LOG_DATA,
+	     "UBX-NAV-TIMELS: source_of_current_leapsecond=%u:%s curr_ls=%d\n",
+	     src_of_curr_ls,src,curr_ls);
+      session->context->leap_seconds = curr_ls;
+      session->context->valid |= LEAP_SECOND_VALID;
+    } /* Valid current leap second */
+    if(valid_time_to_ls_event) {
+      char *src="Unknown";
+      unsigned int src_of_ls_change;
+      unsigned short dateOfLSGpsWn, dateOfLSGpsDn;
+      int lsChange = getsb(buf,11);
+      int timeToLsEvent = getles32(buf,12);
+      src_of_ls_change = getub(buf,10);
+      if(src_of_ls_change < (sizeof srcOfLsChange/(sizeof srcOfLsChange[0])))
+	src = srcOfLsChange[src_of_ls_change];
+      dateOfLSGpsWn = getles16(buf,16);
+      dateOfLSGpsDn = getles16(buf,18);
+      gpsd_log(&session->context->errout, LOG_DATA,
+	 "UBX_NAV_TIMELS: source_of_leapsecond_change=%u:%s "
+	 "leapSecondChage=%d timeToLsEvent=%d\n",
+	 src_of_ls_change,src,lsChange,timeToLsEvent);
+      gpsd_log(&session->context->errout, LOG_DATA,
+	       "UBX_NAV_TIMELS: dateOfLSGpsWn=%d dateOfLSGpsDn=%d\n",
+	       dateOfLSGpsWn,dateOfLSGpsDn);
+      if(timeToLsEvent < 60*60*23 && timeToLsEvent > 0) {
+	if(lsChange == 0)
+	   session->context->leap_notify = LEAP_NOWARNING;
+	else if (lsChange == 1)
+	  session->context->leap_notify = LEAP_ADDSECOND;
+	else if (lsChange == -1)
+	  session->context->leap_notify = LEAP_DELSECOND;
+      } else
+	session->context->leap_notify = LEAP_NOWARNING;
+
+      if (session->context->leap_notify == LEAP_ADDSECOND)
+	gpsd_log(&session->context->errout,LOG_INF,
+		"UBX_NAV_TIMELS: Add leap second today\n");
+      else if (session->context->leap_notify == LEAP_DELSECOND)
+	gpsd_log(&session->context->errout,LOG_INF,
+		 "UBX_NAV_TIMELS: Remove leap second today\n");
+
+      gpsd_log(&session->context->errout, LOG_DATA,
+	       "UBX_NAV_TIMELS: leaving: leap_notify=%d\n",
+	session->context->leap_notify);
+    }
+}
+#endif  /* UBLOXTIMELS_ENABLE */
+
  /**
  * Geodetic position solution message
  * UBX-NAV-POSLLH, Class 1, ID 2
@@ -1767,7 +1874,10 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
 	mask = ubx_msg_nav_timegps(session, &buf[UBX_PREFIX_LEN], data_len);
 	break;
     case UBX_NAV_TIMELS:
-        ubx_msg_nav_timels(session, &buf[UBX_PREFIX_LEN], data_len);
+        gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_TIMELS\n");
+#ifdef UBLOXTIMELS_ENABLE
+	ubx_msg_nav_timels(session, &buf[UBX_PREFIX_LEN],data_len);
+#endif // UBLOXTIMELS_ENABLE
 	break;
     case UBX_NAV_TIMEUTC:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX-NAV-TIMEUTC\n");
@@ -1811,6 +1921,7 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX-RXM-RTCM\n");
 	break;
     case UBX_RXM_SFRB:
+	gpsd_log(&session->context->errout, LOG_DATA, "UBX_RXM_SFRB\n");
 	mask = ubx_rxm_sfrb(session, &buf[UBX_PREFIX_LEN], data_len);
 	break;
     case UBX_RXM_SFRBX:
@@ -2154,11 +2265,19 @@ static void ubx_cfg_prt(struct gps_device_t *session,
 	msg[2] = 0x00;		/* rate */
 	(void)ubx_write(session, 0x06u, 0x01, msg, 3);

+#ifdef UBLOXTIMELS_ENABLE
+	msg[0] = 0x01;		/* class */
+	msg[1] = 0x26;		/* msg id  = UBX_NAV_TIMELS */
+	msg[2] = 0xff;		/* rate */
+#endif /* UBLOXTIMELS_ENABLE */
+	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
+
         /* NAV-SVINFO became NAV-SAT */
 	msg[0] = 0x01;		/* class */
 	msg[1] = 0x30;		/* msg id  = NAV-SVINFO */
 	msg[2] = 0x00;		/* rate */
 	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
+
 	msg[0] = 0x01;		/* class */
 	msg[1] = 0x35;		/* msg id  = NAV-SAT */
 	msg[2] = 0x00;		/* rate */
@@ -2263,6 +2382,13 @@ static void ubx_cfg_prt(struct gps_device_t *session,
 	msg[2] = 0x01;		/* rate */
 	(void)ubx_write(session, 0x06u, 0x01, msg, 3);

+#ifdef UBLOXTIMELS_ENABLE
+	msg[0] = 0x01;		/* class */
+	msg[1] = 0x26;		/* msg id  = UBX_NAV_TIMELS */
+	msg[2] = 0xff;		/* rate */
+	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
+#endif /* UBLOXTIMELS_ENABLE */
+
         /* UBX-NAV-SVINFO deprecated in u-blox 8, gone in u-blox 9.
          * Use UBX-NAV-SAT after u-blox 7 */
 	if (10 > session->driver.ubx.protver) {
--
2.7.4