- SV publisher/subscriber: add support for timestamps with nanosecond resolution

examples/iec61850_9_2_LE_example/iec61850_9_2_LE_example.c

@@ -226,7 +226,7 @@ main(int argc, char** argv)
                 SVPublisher_ASDU_setINT32(asdu, vol4, voltageN);
                 SVPublisher_ASDU_setQuality(asdu, vol4q, q);
 
-                SVPublisher_ASDU_setRefrTm(asdu, Hal_getTimeInMs());
+                SVPublisher_ASDU_setRefrTmNs(asdu, Hal_getTimeInNs());
 
                 SVPublisher_ASDU_setSmpCnt(asdu, (uint16_t) sampleCount);
 

src/sampled_values/sv_publisher.c

@@ -58,12 +58,11 @@ struct sSVPublisher_ASDU {
     uint16_t smpCntLimit;
     uint32_t confRev;
 
-    uint64_t refrTm;
+    Timestamp* refrTm;
     uint8_t smpMod;
     uint16_t smpRate;
 
     uint8_t* smpCntBuf;
-    uint8_t* refrTmBuf;
     uint8_t* smpSynchBuf;
 
     SVPublisher_ASDU _next;
@@ -265,40 +264,6 @@ encodeInt64FixedSize(int64_t value, uint8_t* buffer, int bufPos)
     return bufPos;
 }
 
-static int
-encodeUtcTime(uint64_t timeval, uint8_t* buffer, int bufPos)
-{
-    uint32_t timeval32 = (timeval / 1000LL);
-
-    uint8_t* timeArray = (uint8_t*) &timeval32;
-    uint8_t* valueArray = buffer + bufPos;
-
-#if (ORDER_LITTLE_ENDIAN == 1)
-    valueArray[0] = timeArray[3];
-    valueArray[1] = timeArray[2];
-    valueArray[2] = timeArray[1];
-    valueArray[3] = timeArray[0];
-#else
-    valueArray[0] = timeArray[0];
-    valueArray[1] = timeArray[1];
-    valueArray[2] = timeArray[2];
-    valueArray[3] = timeArray[3];
-#endif
-
-    uint32_t remainder = (timeval % 1000LL);
-    uint32_t fractionOfSecond = (remainder) * 16777 + ((remainder * 216) / 1000);
-
-    /* encode fraction of second */
-    valueArray[4] = ((fractionOfSecond >> 16) & 0xff);
-    valueArray[5] = ((fractionOfSecond >> 8) & 0xff);
-    valueArray[6] = (fractionOfSecond & 0xff);
-
-    /* encode time quality */
-    valueArray[7] = 0x0a; /* 10 bit sub-second time accuracy */
-
-    return bufPos + 8;
-}
-
 SVPublisher
 SVPublisher_createEx(CommParameters* parameters, const char* interfaceId, bool useVlanTag)
 {
@@ -332,7 +297,6 @@ SVPublisher_addASDU(SVPublisher self, const char* svID, const char* datset, uint
     newAsdu->datset = datset;
     newAsdu->confRev = confRev;
     newAsdu->smpCntLimit = UINT16_MAX;
-
     newAsdu->_next = NULL;
 
     /* append new ASDU to list */
@@ -417,8 +381,8 @@ SVPublisher_ASDU_encodeToBuffer(SVPublisher_ASDU self, uint8_t* buffer, int bufP
     /* RefrTm */
     if (self->hasRefrTm) {
         bufPos = BerEncoder_encodeTL(0x84, 8, buffer, bufPos);
-        self->refrTmBuf = buffer + bufPos;
+        self->refrTm = (Timestamp*) (buffer + bufPos);
-        bufPos = encodeUtcTime(self->refrTm, buffer, bufPos);
+        bufPos += 8;
     }
 
     /* SmpSynch */
@@ -724,13 +688,34 @@ SVPublisher_ASDU_enableRefrTm(SVPublisher_ASDU self)
 }
 
 void
-SVPublisher_ASDU_setRefrTm(SVPublisher_ASDU self, uint64_t refrTm)
+SVPublisher_ASDU_setRefrTmNs(SVPublisher_ASDU self, nsSinceEpoch refrTmNs)
+{
+    self->hasRefrTm = true;
+
+    if (self->refrTm) {
+        Timestamp_setTimeInNanoseconds(self->refrTm, refrTmNs);
+        Timestamp_setSubsecondPrecision(self->refrTm, 20);
+    }
+}
+
+void
+SVPublisher_ASDU_setRefrTm(SVPublisher_ASDU self, msSinceEpoch refrTm)
+{
+    self->hasRefrTm = true;
+
+    if (self->refrTm) {
+        Timestamp_setTimeInMilliseconds(self->refrTm, refrTm);
+        Timestamp_setSubsecondPrecision(self->refrTm, 10);
+    }
+}
+
+void
+SVPublisher_ASDU_setRefrTmByTimestamp(SVPublisher_ASDU self, Timestamp* refrTm)
 {
     self->hasRefrTm = true;
-    self->refrTm = refrTm;
 
-    if (self->refrTmBuf != NULL)
+    if (self->refrTm)
-        encodeUtcTime(self->refrTm, self->refrTmBuf, 0);
+        memcpy(self->refrTm, refrTm, 8);
 }
 
 void

src/sampled_values/sv_publisher.h

@@ -329,12 +329,34 @@ LIB61850_API void
 SVPublisher_ASDU_enableRefrTm(SVPublisher_ASDU self);
 
 /**
- * \brief Set the refresh time attribute of the ASDU.
+ * \brief Set the refresh time attribute of the ASDU with nanosecond resolution
  *
  * \param[in] self the Sampled Values ASDU instance.
+ * \param[in] refrTmNs the refresh time value with nanoseconds resolution.
  */
 LIB61850_API void
-SVPublisher_ASDU_setRefrTm(SVPublisher_ASDU self, uint64_t refrTm);
+SVPublisher_ASDU_setRefrTmNs(SVPublisher_ASDU self, nsSinceEpoch refrTmNs);
+
+/**
+ * \brief Set the refresh time attribute of the ASDU with millisecond resolution
+ *
+ * \param[in] self the Sampled Values ASDU instance.
+ * \param[in] refrTmNs the refresh time value with with milliseconds resolution.
+ */
+LIB61850_API void
+SVPublisher_ASDU_setRefrTm(SVPublisher_ASDU self, msSinceEpoch refrTm);
+
+/**
+ * \brief Set the refresh time attribute of the ASDU
+ *
+ * NOTE: Using this function you can control the time quality flags and the
+ * sub second precision (number of valid bits) of the RefrTm value.
+ *
+ * \param[in] self the Sampled Values ASDU instance.
+ * \param[in] refrTm the refresh time value
+ */
+LIB61850_API void
+SVPublisher_ASDU_setRefrTmByTimestamp(SVPublisher_ASDU self, Timestamp* refrTm);
 
 /**
  * \brief Set the sample mode attribute of the ASDU.

src/sampled_values/sv_subscriber.c

@@ -339,7 +339,7 @@ parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)
         if (SVSubscriber_ASDU_hasDatSet(&asdu))
             printf("SV_SUBSCRIBER:     DatSet: %s\n", asdu.datSet);
         if (SVSubscriber_ASDU_hasRefrTm(&asdu))
-            printf("SV_SUBSCRIBER:     RefrTm: %lu\n", SVSubscriber_ASDU_getRefrTmAsMs(&asdu));
+            printf("SV_SUBSCRIBER:     RefrTm[ns]: %lu\n", SVSubscriber_ASDU_getRefrTmAsNs(&asdu));
         if (SVSubscriber_ASDU_hasSmpMod(&asdu))
             printf("SV_SUBSCRIBER:     SmpMod: %d\n", SVSubscriber_ASDU_getSmpMod(&asdu));
         if (SVSubscriber_ASDU_hasSmpRate(&asdu))
@@ -599,8 +599,8 @@ SVSubscriber_ASDU_getSmpCnt(SVSubscriber_ASDU self)
     return retVal;
 }
 
-static uint64_t
+static nsSinceEpoch
-decodeUtcTime(uint8_t* buffer, uint8_t* timeQuality)
+decodeUtcTimeToNsTime(uint8_t* buffer, uint8_t* timeQuality)
 {
     uint32_t timeval32;
 
@@ -609,33 +609,45 @@ decodeUtcTime(uint8_t* buffer, uint8_t* timeQuality)
     timeval32 += buffer[1] * 0x10000;
     timeval32 += buffer[0] * 0x1000000;
 
-    uint32_t msVal;
-
     uint32_t fractionOfSecond;
 
     fractionOfSecond = buffer[6];
     fractionOfSecond += buffer[5] * 0x100;
     fractionOfSecond += buffer[4] * 0x10000;
 
-    msVal = (uint32_t) (((uint64_t) fractionOfSecond * 1000) / 16777215);
+    uint64_t nsVal = fractionOfSecond;
+
+    nsVal = nsVal * 1000000000UL;
+    nsVal = nsVal >> 24;
 
     if (timeQuality != NULL)
         *timeQuality = buffer[7];
 
-    uint64_t timeval64 = (uint64_t) timeval32 * 1000 + (uint64_t) msVal;
+    uint64_t timeval64 = (uint64_t) timeval32 * 1000000000LLU + nsVal;
 
     return timeval64;
 }
 
-uint64_t
+msSinceEpoch
 SVSubscriber_ASDU_getRefrTmAsMs(SVSubscriber_ASDU self)
 {
-    uint64_t msTime = 0;
+    msSinceEpoch msTime = 0;
+
+    if (self->refrTm != NULL)
+        msTime = decodeUtcTimeToNsTime(self->refrTm, NULL);
+
+    return (msTime / 1000000LLU);
+}
+
+nsSinceEpoch
+SVSubscriber_ASDU_getRefrTmAsNs(SVSubscriber_ASDU self)
+{
+    nsSinceEpoch nsTime = 0;
 
     if (self->refrTm != NULL)
-        msTime = decodeUtcTime(self->refrTm, NULL);
+        nsTime = decodeUtcTimeToNsTime(self->refrTm, NULL);
 
-    return msTime;
+    return nsTime;
 }
 
 bool

src/sampled_values/sv_subscriber.h

@@ -376,7 +376,7 @@ LIB61850_API bool
 SVSubscriber_ASDU_hasSmpRate(SVSubscriber_ASDU self);
 
 /**
- * \brief Get the RefrTim value included in SV ASDU as ms timestamp
+ * \brief Get the RefrTim value included in SV ASDU as milliseconds timestamp
  *
  * \param self ASDU object instance
  *
@@ -385,6 +385,16 @@ SVSubscriber_ASDU_hasSmpRate(SVSubscriber_ASDU self);
 LIB61850_API uint64_t
 SVSubscriber_ASDU_getRefrTmAsMs(SVSubscriber_ASDU self);
 
+/**
+ * \brief Get the RefrTim value included in SV ASDU as nanoseconds timestamp
+ *
+ * \param self ASDU object instance
+ *
+ * \return the time value as nanoseconds timestamp or 0 if RefrTm is not present in the SV ASDU
+ */
+LIB61850_API nsSinceEpoch
+SVSubscriber_ASDU_getRefrTmAsNs(SVSubscriber_ASDU self);
+
 /**
  * \brief Get an INT8 data value in the data part of the ASDU
  *