Framework for measuring latency

Building an approach to instrumenting and tracking
the processing latency distribution within the Loc
Tech GNSS system

CRs-fixed: 2684481

Change-Id: I469546fbb02b190e9cb82b7b94cbb63771844fd6

9 files changed, 195 insertions, 7 deletions

diff --git a/core/LocAdapterBase.cpp b/core/LocAdapterBase.cpp
index 6d31778..763426e 100644
--- a/core/LocAdapterBase.cpp
+++ b/core/LocAdapterBase.cpp
@@ -209,4 +209,9 @@ DEFAULT_IMPL(false)
 bool LocAdapterBase::
     reportGnssAdditionalSystemInfoEvent(GnssAdditionalSystemInfo& /*additionalSystemInfo*/)
 DEFAULT_IMPL(false)
+
+void
+LocAdapterBase::reportLatencyInfoEvent(const GnssLatencyInfo& /*gnssLatencyInfo*/)
+DEFAULT_IMPL()
+
 } // namespace loc_core
diff --git a/core/LocAdapterBase.h b/core/LocAdapterBase.h
index 380a613..373add2 100644
--- a/core/LocAdapterBase.h
+++ b/core/LocAdapterBase.h
@@ -198,6 +198,7 @@ public:
     virtual bool reportKlobucharIonoModelEvent(GnssKlobucharIonoModel& ionoModel);
     virtual bool reportGnssAdditionalSystemInfoEvent(
             GnssAdditionalSystemInfo& additionalSystemInfo);
+    virtual void reportLatencyInfoEvent(const GnssLatencyInfo& gnssLatencyInfo);
 };
 
 } // namespace loc_core
diff --git a/core/LocApiBase.cpp b/core/LocApiBase.cpp
index 3f15d69..207849c 100644
--- a/core/LocApiBase.cpp
+++ b/core/LocApiBase.cpp
@@ -561,6 +561,12 @@ void LocApiBase::reportGnssConfig(uint32_t sessionId, const GnssConfig& gnssConf
     TO_ALL_LOCADAPTERS(mLocAdapters[i]->reportGnssConfigEvent(sessionId, gnssConfig));
 }
 
+void LocApiBase::reportLatencyInfo(GnssLatencyInfo& gnssLatencyInfo)
+{
+    // loop through adapters, and deliver to the first handling adapter.
+    TO_ALL_LOCADAPTERS(mLocAdapters[i]->reportLatencyInfoEvent(gnssLatencyInfo));
+}
+
 enum loc_api_adapter_err LocApiBase::
    open(LOC_API_ADAPTER_EVENT_MASK_T /*mask*/)
 DEFAULT_IMPL(LOC_API_ADAPTER_ERR_SUCCESS)
diff --git a/core/LocApiBase.h b/core/LocApiBase.h
index fbfd0bd..0831cc7 100644
--- a/core/LocApiBase.h
+++ b/core/LocApiBase.h
@@ -190,6 +190,7 @@ public:
     void reportKlobucharIonoModel(GnssKlobucharIonoModel& ionoModel);
     void reportGnssAdditionalSystemInfo(GnssAdditionalSystemInfo& additionalSystemInfo);
     void reportGnssConfig(uint32_t sessionId, const GnssConfig& gnssConfig);
+    void reportLatencyInfo(GnssLatencyInfo& gnssLatencyInfo);
 
     // downward calls
     // All below functions are to be defined by adapter specific modules:
diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp
index 85567fb..f306d07 100644
--- a/gnss/GnssAdapter.cpp
+++ b/gnss/GnssAdapter.cpp
@@ -71,6 +71,16 @@ static void agpsCloseResultCb (bool isSuccess, AGpsExtType agpsType, void* userD
 
 typedef const CdfwInterface* (*getCdfwInterface)();
 
+inline bool GnssReportLoggerUtil::isLogEnabled() {
+    return (mLogLatency != nullptr);
+}
+
+inline void GnssReportLoggerUtil::log(const GnssLatencyInfo& gnssLatencyMeasInfo) {
+    if (mLogLatency != nullptr) {
+        mLogLatency(gnssLatencyMeasInfo);
+    }
+}
+
 GnssAdapter::GnssAdapter() :
     LocAdapterBase(0,
                    LocDualContext::getLocFgContext(NULL,
@@ -110,7 +120,8 @@ GnssAdapter::GnssAdapter() :
     mLocSystemInfo{},
     mGnssMbSvIdUsedInPosition{},
     mGnssMbSvIdUsedInPosAvail(false),
-    mPowerState(POWER_STATE_UNKNOWN)
+    mPowerState(POWER_STATE_UNKNOWN),
+    mGnssLatencyInfo{}
 {
     LOC_LOGD("%s]: Constructor %p", __func__, this);
     mLocPositionMode.mode = LOC_POSITION_MODE_INVALID;
@@ -2389,7 +2400,9 @@ GnssAdapter::stopClientSessions(LocationAPI* client)
 void
 GnssAdapter::updateClientsEventMask()
 {
-    LOC_API_ADAPTER_EVENT_MASK_T mask = 0;
+    // need to register for leap second info
+    // for proper nmea generation
+    LOC_API_ADAPTER_EVENT_MASK_T mask = LOC_API_ADAPTER_BIT_LOC_SYSTEM_INFO;
     for (auto it=mClientData.begin(); it != mClientData.end(); ++it) {
         if (it->second.trackingCb != nullptr ||
             it->second.gnssLocationInfoCb != nullptr ||
@@ -2427,7 +2440,6 @@ GnssAdapter::updateClientsEventMask()
         mask |= LOC_API_ADAPTER_BIT_GNSS_SV_POLYNOMIAL_REPORT;
         mask |= LOC_API_ADAPTER_BIT_PARSED_UNPROPAGATED_POSITION_REPORT;
         mask |= LOC_API_ADAPTER_BIT_GNSS_SV_EPHEMERIS_REPORT;
-        mask |= LOC_API_ADAPTER_BIT_LOC_SYSTEM_INFO;
         mask |= LOC_API_ADAPTER_BIT_EVENT_REPORT_INFO;
 
         // Nhz measurement bit is set based on callback from loc eng hub
@@ -2448,9 +2460,13 @@ GnssAdapter::updateClientsEventMask()
         mask |= LOC_API_ADAPTER_BIT_REQUEST_WIFI;
     }
 
-    // need to register for leap second info
-    // for proper nmea generation
-    mask |= LOC_API_ADAPTER_BIT_LOC_SYSTEM_INFO;
+    // Enable the latency report
+    if (mask & LOC_API_ADAPTER_BIT_GNSS_MEASUREMENT) {
+        if (mLogger.isLogEnabled()) {
+            mask |= LOC_API_ADAPTER_BIT_LATENCY_INFORMATION;
+        }
+    }
+
     updateEvtMask(mask, LOC_REGISTRATION_MASK_SET);
 }
 
@@ -3687,6 +3703,61 @@ bool GnssAdapter::needToGenerateNmeaReport(const uint32_t &gpsTimeOfWeekMs,
     return retVal;
 }
 
+void
+GnssAdapter::logLatencyInfo()
+{
+    if (0 != mGnssLatencyInfo.meQtimer2) {
+        mGnssLatencyInfo.hlosQtimer5 = getQTimerTickCount();
+
+        if (0 == mGnssLatencyInfo.hlosQtimer3) {
+            /* if SPE from engine hub is not reported then hlosQtimer3 = 0, set it
+            equal to hlosQtimer2 to make sense */
+            LOC_LOGv("hlosQtimer3 is 0, setting it to hlosQtimer2");
+            mGnssLatencyInfo.hlosQtimer3 = mGnssLatencyInfo.hlosQtimer2;
+        }
+        if (0 == mGnssLatencyInfo.hlosQtimer4) {
+            /* if PPE from engine hub is not reported then hlosQtimer4 = 0, set it
+            equal to hlosQtimer3 to make sense */
+            LOC_LOGv("hlosQtimer4 is 0, setting it to hlosQtimer3");
+            mGnssLatencyInfo.hlosQtimer4 = mGnssLatencyInfo.hlosQtimer3;
+        }
+        if (mGnssLatencyInfo.hlosQtimer4 < mGnssLatencyInfo.hlosQtimer3) {
+            /* hlosQtimer3 is timestamped when SPE from engine hub is reported,
+            and hlosQtimer4 is timestamped when PPE from engine hub is reported.
+            The order is random though, hence making sure the timestamps are sorted */
+            LOC_LOGv("hlosQtimer4 is < hlosQtimer3, swapping them");
+            std::swap(mGnssLatencyInfo.hlosQtimer3, mGnssLatencyInfo.hlosQtimer4);
+        }
+        LOC_LOGv("meQtimer1=%" PRIi64 " "
+            "meQtimer2=%" PRIi64 " "
+            "meQtimer3=%" PRIi64 " "
+            "peQtimer1=%" PRIi64 " "
+            "peQtimer2=%" PRIi64 " "
+            "peQtimer3=%" PRIi64 " "
+            "smQtimer1=%" PRIi64 " "
+            "smQtimer2=%" PRIi64 " "
+            "smQtimer3=%" PRIi64 " "
+            "locMwQtimer=%" PRIi64 " "
+            "hlosQtimer1=%" PRIi64 " "
+            "hlosQtimer2=%" PRIi64 " "
+            "hlosQtimer3=%" PRIi64 " "
+            "hlosQtimer4=%" PRIi64 " "
+            "hlosQtimer5=%" PRIi64 " ",
+            mGnssLatencyInfo.meQtimer1, mGnssLatencyInfo.meQtimer2,
+            mGnssLatencyInfo.meQtimer3, mGnssLatencyInfo.peQtimer1,
+            mGnssLatencyInfo.peQtimer2, mGnssLatencyInfo.peQtimer3,
+            mGnssLatencyInfo.smQtimer1, mGnssLatencyInfo.smQtimer2,
+            mGnssLatencyInfo.smQtimer3, mGnssLatencyInfo.locMwQtimer,
+            mGnssLatencyInfo.hlosQtimer1, mGnssLatencyInfo.hlosQtimer2,
+            mGnssLatencyInfo.hlosQtimer3, mGnssLatencyInfo.hlosQtimer4,
+            mGnssLatencyInfo.hlosQtimer5);
+        mLogger.log(mGnssLatencyInfo);
+        mGnssLatencyInfo.hlosQtimer3 = 0;
+        mGnssLatencyInfo.hlosQtimer4 = 0;
+        mGnssLatencyInfo.hlosQtimer5 = 0;
+    }
+}
+
 // only fused report (when engine hub is enabled) or
 // SPE report (when engine hub is disabled) will reach this function
 void
@@ -3715,7 +3786,7 @@ GnssAdapter::reportPosition(const UlpLocation& ulpLocation,
         GnssLocationInfoNotification locationInfo = {};
         convertLocationInfo(locationInfo, locationExtended);
         convertLocation(locationInfo.location, ulpLocation, locationExtended, techMask);
-
+        logLatencyInfo();
         for (auto it=mClientData.begin(); it != mClientData.end(); ++it) {
             if ((nullptr != it->second.engineLocationsInfoCb) &&
                 (false == initEngHubProxy())) {
@@ -3773,6 +3844,34 @@ GnssAdapter::reportPosition(const UlpLocation& ulpLocation,
 }
 
 void
+GnssAdapter::reportLatencyInfoEvent(const GnssLatencyInfo& gnssLatencyInfo)
+{
+    struct MsgReportLatencyInfo : public LocMsg {
+        GnssAdapter& mAdapter;
+        GnssLatencyInfo mGnssLatencyInfo;
+        inline MsgReportLatencyInfo(GnssAdapter& adapter,
+            const GnssLatencyInfo& gnssLatencyInfo) :
+            mGnssLatencyInfo(gnssLatencyInfo),
+            mAdapter(adapter) {}
+        inline virtual void proc() const {
+            mAdapter.mGnssLatencyInfo.meQtimer1 = mGnssLatencyInfo.meQtimer1;
+            mAdapter.mGnssLatencyInfo.meQtimer2 = mGnssLatencyInfo.meQtimer2;
+            mAdapter.mGnssLatencyInfo.meQtimer3 = mGnssLatencyInfo.meQtimer3;
+            mAdapter.mGnssLatencyInfo.peQtimer1 = mGnssLatencyInfo.peQtimer1;
+            mAdapter.mGnssLatencyInfo.peQtimer2 = mGnssLatencyInfo.peQtimer2;
+            mAdapter.mGnssLatencyInfo.peQtimer3 = mGnssLatencyInfo.peQtimer3;
+            mAdapter.mGnssLatencyInfo.smQtimer1 = mGnssLatencyInfo.smQtimer1;
+            mAdapter.mGnssLatencyInfo.smQtimer2 = mGnssLatencyInfo.smQtimer2;
+            mAdapter.mGnssLatencyInfo.smQtimer3 = mGnssLatencyInfo.smQtimer3;
+            mAdapter.mGnssLatencyInfo.locMwQtimer = mGnssLatencyInfo.locMwQtimer;
+            mAdapter.mGnssLatencyInfo.hlosQtimer1 = mGnssLatencyInfo.hlosQtimer1;
+            mAdapter.mGnssLatencyInfo.hlosQtimer2 = mGnssLatencyInfo.hlosQtimer2;
+        }
+    };
+    sendMsg(new MsgReportLatencyInfo(*this, gnssLatencyInfo));
+}
+
+void
 GnssAdapter::reportEnginePositions(unsigned int count,
                                    const EngineLocationInfo* locationArr)
 {
@@ -3805,6 +3904,19 @@ GnssAdapter::reportEnginePositions(unsigned int count,
         }
     }
 
+    const EngineLocationInfo* engLocation = locationArr;
+    LOC_LOGv("engLocation->locationExtended.locOutputEngType=%d",
+             engLocation->locationExtended.locOutputEngType);
+    if ((GPS_LOCATION_EXTENDED_HAS_OUTPUT_ENG_TYPE & engLocation->locationExtended.flags) &&
+        (LOC_OUTPUT_ENGINE_SPE == engLocation->locationExtended.locOutputEngType)) {
+        mGnssLatencyInfo.hlosQtimer3 = getQTimerTickCount();
+        LOC_LOGv("SPE mGnssLatencyInfo.hlosQtimer3=%" PRIi64 " ", mGnssLatencyInfo.hlosQtimer3);
+    }
+    if ((GPS_LOCATION_EXTENDED_HAS_OUTPUT_ENG_TYPE & engLocation->locationExtended.flags) &&
+        (LOC_OUTPUT_ENGINE_PPE == engLocation->locationExtended.locOutputEngType)) {
+        mGnssLatencyInfo.hlosQtimer4 = getQTimerTickCount();
+        LOC_LOGv("PPE mGnssLatencyInfo.hlosQtimer4=%" PRIi64 " ", mGnssLatencyInfo.hlosQtimer4);
+    }
     if (needReportEnginePositions) {
         for (auto it=mClientData.begin(); it != mClientData.end(); ++it) {
             if (nullptr != it->second.engineLocationsInfoCb) {
diff --git a/gnss/GnssAdapter.h b/gnss/GnssAdapter.h
index 794301f..acae728 100644
--- a/gnss/GnssAdapter.h
+++ b/gnss/GnssAdapter.h
@@ -37,6 +37,7 @@
 #include <Agps.h>
 #include <SystemStatus.h>
 #include <XtraSystemStatusObserver.h>
+#include <loc_misc_utils.h>
 
 #define MAX_URL_LEN 256
 #define NMEA_SENTENCE_MAX_LENGTH 200
@@ -169,6 +170,23 @@ struct CdfwInterface {
     void (*reportUsable)(QDgnssListenerHDL handle, bool usable);
 };
 
+class GnssReportLoggerUtil {
+public:
+    typedef void (*LogGnssLatency)(const GnssLatencyInfo& gnssLatencyMeasInfo);
+
+    GnssReportLoggerUtil() : mLogLatency(nullptr) {
+        const char* libname = "liblocdiagiface.so";
+        void* libHandle = nullptr;
+        mLogLatency = (LogGnssLatency)dlGetSymFromLib(libHandle, libname, "LogGnssLatency");
+    }
+
+    bool isLogEnabled();
+    void log(const GnssLatencyInfo& gnssLatencyMeasInfo);
+
+private:
+    LogGnssLatency mLogLatency;
+};
+
 class GnssAdapter : public LocAdapterBase {
 
     /* ==== Engine Hub ===================================================================== */
@@ -233,6 +251,8 @@ class GnssAdapter : public LocAdapterBase {
 
     /* === Misc ===================================================================== */
     BlockCPIInfo mBlockCPIInfo;
+    GnssLatencyInfo mGnssLatencyInfo;
+    GnssReportLoggerUtil mLogger;
 
     /* === Misc callback from QMI LOC API ============================================== */
     GnssEnergyConsumedCallback mGnssEnergyConsumedCb;
@@ -251,6 +271,7 @@ class GnssAdapter : public LocAdapterBase {
     inline void initOdcpi(const OdcpiRequestCallback& callback);
     inline void injectOdcpi(const Location& location);
     inline void setNmeaReportRateConfig();
+    void logLatencyInfo();
 
 public:
 
@@ -455,6 +476,7 @@ public:
     virtual bool reportKlobucharIonoModelEvent(GnssKlobucharIonoModel& ionoModel);
     virtual bool reportGnssAdditionalSystemInfoEvent(
             GnssAdditionalSystemInfo& additionalSystemInfo);
+    virtual void reportLatencyInfoEvent(const GnssLatencyInfo& gnssLatencyInfo);
 
     /* ======== UTILITIES ================================================================= */
     bool needReport(const UlpLocation& ulpLocation,
diff --git a/location/LocationDataTypes.h b/location/LocationDataTypes.h
index a98c8db..b2a6a8f 100644
--- a/location/LocationDataTypes.h
+++ b/location/LocationDataTypes.h
@@ -1806,4 +1806,22 @@ typedef struct {
     engineLocationsInfoCallback engineLocationsInfoCb;     // optional
 } LocationCallbacks;
 
+typedef struct {
+    uint64_t meQtimer1;
+    uint64_t meQtimer2;
+    uint64_t meQtimer3;
+    uint64_t peQtimer1;
+    uint64_t peQtimer2;
+    uint64_t peQtimer3;
+    uint64_t smQtimer1;
+    uint64_t smQtimer2;
+    uint64_t smQtimer3;
+    uint64_t locMwQtimer;
+    uint64_t hlosQtimer1;
+    uint64_t hlosQtimer2;
+    uint64_t hlosQtimer3;
+    uint64_t hlosQtimer4;
+    uint64_t hlosQtimer5;
+} GnssLatencyInfo;
+
 #endif /* LOCATIONDATATYPES_H */
diff --git a/utils/gps_extended_c.h b/utils/gps_extended_c.h
index b6b1213..0df78f9 100644
--- a/utils/gps_extended_c.h
+++ b/utils/gps_extended_c.h
@@ -989,6 +989,7 @@ enum loc_api_adapter_event_index {
     LOC_API_ADAPTER_LOC_SYSTEM_INFO,                   // Location system info event
     LOC_API_ADAPTER_GNSS_NHZ_MEASUREMENT_REPORT,       // GNSS SV nHz measurement report
     LOC_API_ADAPTER_EVENT_REPORT_INFO,                 // Event report info
+    LOC_API_ADAPTER_LATENCY_INFORMATION_REPORT,       // Latency information report
     LOC_API_ADAPTER_EVENT_MAX
 };
 
@@ -1031,6 +1032,7 @@ enum loc_api_adapter_event_index {
 #define LOC_API_ADAPTER_BIT_LOC_SYSTEM_INFO                  (1ULL<<LOC_API_ADAPTER_LOC_SYSTEM_INFO)
 #define LOC_API_ADAPTER_BIT_GNSS_NHZ_MEASUREMENT             (1ULL<<LOC_API_ADAPTER_GNSS_NHZ_MEASUREMENT_REPORT)
 #define LOC_API_ADAPTER_BIT_EVENT_REPORT_INFO                (1ULL<<LOC_API_ADAPTER_EVENT_REPORT_INFO)
+#define LOC_API_ADAPTER_BIT_LATENCY_INFORMATION              (1ULL<<LOC_API_ADAPTER_LATENCY_INFORMATION_REPORT)
 
 typedef uint64_t LOC_API_ADAPTER_EVENT_MASK_T;
 
diff --git a/utils/loc_misc_utils.h b/utils/loc_misc_utils.h
index 062ec20..d6cfcf5 100644
--- a/utils/loc_misc_utils.h
+++ b/utils/loc_misc_utils.h
@@ -261,4 +261,25 @@ SIDE EFFECTS
 void loc_convert_velocity_gnss_to_vrp(float enuVelocity[3], float rollPitchYaw[3],
                                       float rollPitchYawRate[3], float leverArm[3]);
 
+/*===========================================================================
+FUNCTION qTimerTicksToNanos
+
+DESCRIPTION
+Transform from ticks to nanoseconds, clock is 19.2 MHz
+so the formula would be qtimer(ns) = (ticks * 1000000000) / 19200000
+or simplified qtimer(ns) = (ticks * 10000) / 192.
+
+DEPENDENCIES
+N/A
+
+RETURN VALUE
+Qtimer value in nanoseconds
+
+SIDE EFFECTS
+N/A
+===========================================================================*/
+inline uint64_t qTimerTicksToNanos(double qTimer) {
+    return (uint64_t((qTimer * double(10000ull)) / (double)192ull));
+}
+
 #endif //_LOC_MISC_UTILS_H_