AP_HAL_Linux: Perf: rework integration with other tools

The idea is to leave the internal perf enabled all the time, like it is
in PX4, and then allow the integration with lttng on top. Next step
would be to runtime enable/disable only the perf counters we are
interested in.

This also changes the structure so it's easy to allow another thread to
pull data from the Perf object. A rw lock protects from addition of new
counters and an atomic unsigned int allows other threads to do a
lockless copy of the data.

In order for this to work the allocation was changed to use a single
memory pool instead of returning a calloc'ed data for each perf counter.
Since most of our counters are of ' elapsed' type, don't bother using a
smaller struct for the 'count' type

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -55,5 +55,4 @@ namespace Linux {
     class VideoIn;
     class OpticalFlow_Onboard;
     class Flow_PX4;
-    class Perf_Lttng;
 }

libraries/AP_HAL_Linux/Perf.cpp

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 /*
- * Copyright (C) 2015  Intel Corporation. All rights reserved.
+ * Copyright (C) 2016  Intel Corporation. All rights reserved.
  *
  * This file is free software: you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the
@@ -15,42 +15,22 @@
  * You should have received a copy of the GNU General Public License along
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-#ifndef HAVE_LTTNG_UST
-
-#include <limits.h>
 #include <time.h>
+#include <vector>
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 
 #include "AP_HAL_Linux.h"
 #include "Util.h"
+#include "Perf.h"
+#include "Perf_Lttng.h"
 
 using namespace Linux;
 
-struct perf_counter_base_t {
-    const char *name;
-    enum Util::perf_counter_type type;
-};
+static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
 
-struct perf_counter_count_t {
-    struct perf_counter_base_t base;
-    uint64_t count;
-};
-
-struct perf_counter_elapsed_t {
-    struct perf_counter_base_t base;
-    uint64_t count;
-    /* Everything below is in nanoseconds */
-    uint64_t start;
-    uint64_t total;
-    uint64_t least;
-    uint64_t most;
-    double mean;
-    double m2;
-};
-
-static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+Perf *Perf::_instance;
 
 static inline uint64_t now_nsec()
 {
@@ -59,96 +39,92 @@ static inline uint64_t now_nsec()
     return ts.tv_nsec + (ts.tv_sec * NSEC_PER_SEC);
 }
 
-Util::perf_counter_t Util::perf_alloc(perf_counter_type type, const char *name)
+Perf *Perf::get_instance()
 {
-    struct perf_counter_base_t *base;
-
-    switch(type) {
-    case PC_COUNT: {
-        struct perf_counter_count_t *count;
-        count = (struct perf_counter_count_t *)calloc(1, sizeof(struct perf_counter_count_t));
-        if (!count) {
-            return nullptr;
-        }
-
-        base = &count->base;
-        break;
-    }
-    case PC_ELAPSED: {
-        struct perf_counter_elapsed_t *elapsed;
-        elapsed = (struct perf_counter_elapsed_t *)calloc(1, sizeof(struct perf_counter_elapsed_t));
-        if (!elapsed) {
-            return nullptr;
-        }
-
-        elapsed->least = ULONG_MAX;
-
-        base = &elapsed->base;
-        break;
-    }
-    default:
-    case PC_INTERVAL: {
-        /*
-         * Not implemented now because it is not used even on PX4 specific
-         * code and by looking at PX4 implementation without perf_reset()
-         * the average is broken.
-         */
-        return nullptr;
-    }
+    if (!_instance) {
+        _instance = new Perf();
     }
 
-    base->name = name;
-    base->type = type;
-    return (perf_counter_t)base;
+    return _instance;
 }
 
-void Util::perf_begin(perf_counter_t perf)
+Perf::Perf()
 {
-    struct perf_counter_elapsed_t *perf_elapsed = (struct perf_counter_elapsed_t *)perf;
-
-    if (perf_elapsed == NULL) {
-        return;
-    }
-    if (perf_elapsed->base.type != PC_ELAPSED) {
-        hal.console->printf("perf_begin() called over a perf_counter_t(%s) that"
-                            " is not of the PC_ELAPSED type.\n",
-                            perf_elapsed->base.name);
-        return;
+    if (pthread_rwlock_init(&_perf_counters_lock, nullptr) != 0) {
+        AP_HAL::panic("Perf: fail to initialize rw lock");
     }
 
-    perf_elapsed->start = now_nsec();
+    /* TODO: this number should come from vehicle code - just estimate the
+     * number of perf counters for now; if we grow more, it will just
+     * reallocate the memory pool */
+    _perf_counters.reserve(50);
 }
 
-void Util::perf_end(perf_counter_t perf)
+void Perf::begin(Util::perf_counter_t pc)
 {
-    struct perf_counter_elapsed_t *perf_elapsed = (struct perf_counter_elapsed_t *)perf;
+    uintptr_t idx = (uintptr_t)pc;
 
-    if (perf_elapsed == NULL) {
+    if (idx >= _perf_counters.size()) {
         return;
     }
 
-    if (perf_elapsed->base.type != PC_ELAPSED) {
-        hal.console->printf("perf_end() called over a perf_counter_t(%s) "
-                            "that is not of the PC_ELAPSED type.\n",
-                            perf_elapsed->base.name);
-        return;
-    }
-    if (perf_elapsed->start == 0) {
-        hal.console->printf("perf_end() called before an perf_begin() on %s.\n",
-                            perf_elapsed->base.name);
+    Perf_Counter &perf = _perf_counters[idx];
+    if (perf.type != Util::PC_ELAPSED) {
+        hal.console->printf("perf_begin() called on perf_counter_t(%s) that"
+                            " is not of PC_ELAPSED type.\n",
+                            perf.name);
         return;
     }
 
-    const uint64_t elapsed = now_nsec() - perf_elapsed->start;
-    perf_elapsed->count++;
-    perf_elapsed->total += elapsed;
-
-    if (perf_elapsed->least > elapsed) {
-        perf_elapsed->least =  elapsed;
+    if (perf.start != 0) {
+        hal.console->printf("perf_begin() called twice on perf_counter_t(%s)\n",
+                            perf.name);
+        return;
     }
 
-    if (perf_elapsed->most < elapsed) {
-        perf_elapsed->most = elapsed;
+    _update_count++;
+
+    perf.start = now_nsec();
+
+#ifdef HAVE_LTTNG_UST
+    perf.lttng.begin();
+#endif
+}
+
+void Perf::end(Util::perf_counter_t pc)
+{
+    uintptr_t idx = (uintptr_t)pc;
+
+    if (idx >= _perf_counters.size()) {
+        return;
+    }
+
+    Perf_Counter &perf = _perf_counters[idx];
+    if (perf.type != Util::PC_ELAPSED) {
+        hal.console->printf("perf_begin() called on perf_counter_t(%s) that"
+                            " is not of PC_ELAPSED type.\n",
+                            perf.name);
+        return;
+    }
+
+    if (perf.start == 0) {
+        hal.console->printf("perf_begin() called before begin() on perf_counter_t(%s)\n",
+                            perf.name);
+        return;
+    }
+
+    _update_count++;
+
+    const uint64_t elapsed = now_nsec() - perf.start;
+    perf.count++;
+    perf.total += elapsed;
+
+    if (perf.least > elapsed) {
+        perf.least = elapsed;
+    }
+
+    if (perf.most < elapsed) {
+        perf.most = elapsed;
     }
 
     /*
@@ -156,29 +132,55 @@ void Util::perf_end(perf_counter_t perf)
      * Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
      * Same implementation of PX4.
      */
-    const double delta_intvl = elapsed - perf_elapsed->mean;
-    perf_elapsed->mean += (delta_intvl / perf_elapsed->count);
-    perf_elapsed->m2 += (delta_intvl * (elapsed - perf_elapsed->mean));
-
-    perf_elapsed->start = 0;
-}
-
-void Util::perf_count(perf_counter_t perf)
-{
-    struct perf_counter_count_t *perf_counter = (struct perf_counter_count_t *)perf;
-
-    if (perf_counter == NULL) {
-        return;
-    }
-
-    if (perf_counter->base.type != PC_COUNT) {
-        hal.console->printf("perf_count() called over a perf_counter_t(%s) "
-                            "that is not of the PC_COUNT type.\n",
-                            perf_counter->base.name);
-        return;
-    }
-
-    perf_counter->count++;
-}
+    const double delta_intvl = elapsed - perf.mean;
+    perf.mean += (delta_intvl / perf.count);
+    perf.m2 += (delta_intvl * (elapsed - perf.mean));
+    perf.start = 0;
 
+#ifdef HAVE_LTTNG_UST
+    perf.lttng.end();
 #endif
+}
+
+void Perf::count(Util::perf_counter_t pc)
+{
+    uintptr_t idx = (uintptr_t)pc;
+
+    if (idx >= _perf_counters.size()) {
+        return;
+    }
+
+    Perf_Counter &perf = _perf_counters[idx];
+    if (perf.type != Util::PC_COUNT) {
+        hal.console->printf("perf_begin() called on perf_counter_t(%s) that"
+                            " is not of PC_COUNT type.\n",
+                            perf.name);
+        return;
+    }
+
+    _update_count++;
+    perf.count++;
+
+#ifdef HAVE_LTTNG_UST
+    perf.lttng.count();
+#endif
+
+}
+
+Util::perf_counter_t Perf::add(Util::perf_counter_type type, const char *name)
+{
+    if (type != Util::PC_COUNT && type != Util::PC_ELAPSED) {
+        /*
+         * Other perf counters not implemented for now since they are not
+         * used anywhere.
+         */
+        return (Util::perf_counter_t)(uintptr_t) -1;
+    }
+
+    pthread_rwlock_wrlock(&_perf_counters_lock);
+    Util::perf_counter_t pc = (Util::perf_counter_t) _perf_counters.size();
+    _perf_counters.emplace_back(type, name);
+    pthread_rwlock_unlock(&_perf_counters_lock);
+
+    return pc;
+}

libraries/AP_HAL_Linux/Perf.h

@@ -0,0 +1,97 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2016  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <atomic>
+#include <limits.h>
+#include <pthread.h>
+#include <vector>
+
+#include "AP_HAL_Linux.h"
+#include "Perf_Lttng.h"
+#include "Thread.h"
+#include "Util.h"
+
+namespace Linux {
+
+class Perf_Counter {
+    using perf_counter_type = AP_HAL::Util::perf_counter_type;
+    using perf_counter_t = AP_HAL::Util::perf_counter_t;
+
+public:
+    Perf_Counter(perf_counter_type type_, const char *name_)
+        : name{name_}
+        , type{type_}
+#ifdef HAVE_LTTNG_UST
+        , lttng{name_}
+#endif
+        , least{ULONG_MAX}
+    {
+    }
+
+    const char *name;
+#ifdef HAVE_LTTNG_UST
+    Perf_Lttng lttng;
+#endif
+
+    perf_counter_type type;
+
+    uint64_t count;
+
+    /* Everything below is in nanoseconds */
+    uint64_t start;
+    uint64_t total;
+    uint64_t least;
+    uint64_t most;
+
+    double mean;
+    double m2;
+};
+
+class Perf {
+    using perf_counter_type = AP_HAL::Util::perf_counter_type;
+    using perf_counter_t = AP_HAL::Util::perf_counter_t;
+
+public:
+    ~Perf();
+
+    static Perf *get_instance();
+
+    perf_counter_t add(perf_counter_type type, const char *name);
+
+    void begin(perf_counter_t pc);
+    void end(perf_counter_t pc);
+    void count(perf_counter_t pc);
+
+    unsigned int get_update_count() { return _update_count; }
+
+private:
+    static Perf *_instance;
+
+    Perf();
+
+    std::vector<Perf_Counter> _perf_counters;
+
+    /* synchronize addition of new perf counters */
+    pthread_rwlock_t _perf_counters_lock;
+
+    /* allow to check if memory pool has changed */
+    std::atomic<unsigned int> _update_count;
+};
+
+}

libraries/AP_HAL_Linux/Perf_Lttng.cpp

@@ -24,66 +24,27 @@
 #include "AP_HAL_Linux.h"
 #include "Perf_Lttng_TracePoints.h"
 #include "Perf_Lttng.h"
-#include "Util.h"
-
-#pragma GCC diagnostic ignored "-Wcast-align"
 
 using namespace Linux;
 
-Perf_Lttng::Perf_Lttng(AP_HAL::Util::perf_counter_type type, const char *name)
-    : _type(type)
+Perf_Lttng::Perf_Lttng(const char *name)
 {
     strncpy(_name, name, MAX_TRACEPOINT_NAME_LEN);
 }
 
 void Perf_Lttng::begin()
 {
-    if (_type != AP_HAL::Util::PC_ELAPSED) {
-        return;
-    }
     tracepoint(ardupilot, begin, _name);
 }
 
 void Perf_Lttng::end()
 {
-    if (_type != AP_HAL::Util::PC_ELAPSED) {
-        return;
-    }
     tracepoint(ardupilot, end, _name);
 }
 
 void Perf_Lttng::count()
 {
-    if (_type != AP_HAL::Util::PC_COUNT) {
-        return;
-    }
     tracepoint(ardupilot, count, _name, ++_count);
 }
 
-Util::perf_counter_t Util::perf_alloc(perf_counter_type type, const char *name)
-{
-    return new Linux::Perf_Lttng(type, name);
-}
-
-void Util::perf_begin(perf_counter_t perf)
-{
-    Linux::Perf_Lttng *perf_lttng = (Linux::Perf_Lttng *)perf;
-
-    perf_lttng->begin();
-}
-
-void Util::perf_end(perf_counter_t perf)
-{
-    Linux::Perf_Lttng *perf_lttng = (Linux::Perf_Lttng *)perf;
-
-    perf_lttng->end();
-}
-
-void Util::perf_count(perf_counter_t perf)
-{
-    Linux::Perf_Lttng *perf_lttng = (Linux::Perf_Lttng *)perf;
-
-    perf_lttng->count();
-}
-
 #endif

libraries/AP_HAL_Linux/Perf_Lttng.h

@@ -12,21 +12,25 @@
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-
 #pragma once
-#include "AP_HAL_Linux.h"
+
 #include <AP_HAL/Util.h>
 
+#include "AP_HAL_Linux.h"
+
 #define MAX_TRACEPOINT_NAME_LEN 128
 
-class Linux::Perf_Lttng {
+namespace Linux {
+
+class Perf_Lttng {
 public:
-    Perf_Lttng(enum AP_HAL::Util::perf_counter_type type, const char *name);
+    Perf_Lttng(const char *name);
     void begin();
     void end();
     void count();
 private:
     char _name[MAX_TRACEPOINT_NAME_LEN];
     uint64_t _count;
-    enum AP_HAL::Util::perf_counter_type _type;
 };
+
+}

libraries/AP_HAL_Linux/Perf_Lttng_TracePoints.h

@@ -62,4 +62,3 @@ TRACEPOINT_EVENT(
 #endif
 
 #include <lttng/tracepoint-event.h>
-

libraries/AP_HAL_Linux/Util.h

@@ -4,6 +4,7 @@
 #include <AP_HAL/AP_HAL.h>
 
 #include "AP_HAL_Linux_Namespace.h"
+#include "Perf.h"
 #include "ToneAlarm.h"
 #include "Semaphores.h"
 
@@ -63,10 +64,25 @@ public:
      */
     int read_file(const char *path, const char *fmt, ...) FMT_SCANF(3, 4);
 
-    perf_counter_t perf_alloc(perf_counter_type t, const char *name) override;
-    void perf_begin(perf_counter_t perf) override;
-    void perf_end(perf_counter_t perf) override;
-    void perf_count(perf_counter_t perf) override;
+    perf_counter_t perf_alloc(enum perf_counter_type t, const char *name) override
+    {
+        return Perf::get_instance()->add(t, name);
+    }
+
+    void perf_begin(perf_counter_t perf) override
+    {
+        return Perf::get_instance()->begin(perf);
+    }
+
+    void perf_end(perf_counter_t perf) override
+    {
+        return Perf::get_instance()->end(perf);
+    }
+
+    void perf_count(perf_counter_t perf) override
+    {
+        return Perf::get_instance()->count(perf);
+    }
 
     // create a new semaphore
     AP_HAL::Semaphore *new_semaphore(void) override { return new Linux::Semaphore; }