From b8e3e549c7338da44c46fa37a9e483b532d9412b Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Fri, 27 May 2016 09:41:32 -0300
Subject: [PATCH] AP_HAL_Linux: Perf: add debug method

Test code for integration with another thread to pull data from internal
perf counters.  Since we are using the timer thread here, there's no
retry mechanism and we only print that data can be corrupted.
---
 libraries/AP_HAL_Linux/Perf.cpp | 44 +++++++++++++++++++++++++++++++++
 libraries/AP_HAL_Linux/Perf.h   |  4 +++
 2 files changed, 48 insertions(+)

diff --git a/libraries/AP_HAL_Linux/Perf.cpp b/libraries/AP_HAL_Linux/Perf.cpp
index 89b310b060..134b7bd494 100644
--- a/libraries/AP_HAL_Linux/Perf.cpp
+++ b/libraries/AP_HAL_Linux/Perf.cpp
@@ -15,6 +15,7 @@
  * You should have received a copy of the GNU General Public License along
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
+#include <stdio.h>
 #include <time.h>
 #include <vector>
 
@@ -48,6 +49,45 @@ Perf *Perf::get_instance()
     return _instance;
 }
 
+void Perf::_debug_counters()
+{
+    uint64_t now = AP_HAL::millis64();
+
+    if (now - _last_debug_msec < 5000) {
+        return;
+    }
+
+    pthread_rwlock_rdlock(&_perf_counters_lock);
+    unsigned int uc = _update_count;
+    auto v = _perf_counters;
+    pthread_rwlock_unlock(&_perf_counters_lock);
+
+    if (uc != _update_count) {
+        fprintf(stderr, "WARNING!! potentially wrong counters!!!");
+    }
+
+    for (auto &c : v) {
+        if (!c.count) {
+            fprintf(stderr, "%-30s\t"
+                    "(no events)\n", c.name);
+        } else if (c.type == Util::PC_ELAPSED) {
+            fprintf(stderr, "%-30s\t"
+                    "count: %llu\t"
+                    "min: %llu\t"
+                    "max: %llu\t"
+                    "avg: %.4f\t"
+                    "stddev: %.4f\n",
+                    c.name, c.count, c.least, c.most, c.mean, sqrt(c.m2));
+        } else {
+            fprintf(stderr, "%-30s\t"
+                    "count: %llu\n",
+                    c.name, c.count);
+        }
+    }
+
+    _last_debug_msec = now;
+}
+
 Perf::Perf()
 {
     if (pthread_rwlock_init(&_perf_counters_lock, nullptr) != 0) {
@@ -58,6 +98,10 @@ Perf::Perf()
      * number of perf counters for now; if we grow more, it will just
      * reallocate the memory pool */
     _perf_counters.reserve(50);
+
+#ifdef DEBUG_PERF
+    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&Perf::_debug_counters, void));
+#endif
 }
 
 void Perf::begin(Util::perf_counter_t pc)
diff --git a/libraries/AP_HAL_Linux/Perf.h b/libraries/AP_HAL_Linux/Perf.h
index eb16051ace..7ad8474176 100644
--- a/libraries/AP_HAL_Linux/Perf.h
+++ b/libraries/AP_HAL_Linux/Perf.h
@@ -80,6 +80,10 @@ private:
 
     Perf();
 
+    void _debug_counters();
+
+    uint64_t _last_debug_msec;
+
     std::vector<Perf_Counter> _perf_counters;
 
     /* synchronize addition of new perf counters */