diff --git a/libraries/AP_HAL_Linux/Perf.cpp b/libraries/AP_HAL_Linux/Perf.cpp
new file mode 100644
index 0000000000..e4c91c03f2
--- /dev/null
+++ b/libraries/AP_HAL_Linux/Perf.cpp
@@ -0,0 +1,177 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2015 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 .
+ */
+
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include
+#include
+
+#include
+
+#include "AP_HAL_Linux.h"
+#include "Util.h"
+
+using namespace Linux;
+
+struct perf_counter_base_t {
+ const char *name;
+ enum Util::perf_counter_type type;
+};
+
+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();
+
+Util::perf_counter_t Util::perf_alloc(perf_counter_type type, const char *name)
+{
+ 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;
+ }
+ }
+
+ base->name = name;
+ base->type = type;
+ return (perf_counter_t)base;
+}
+
+static inline uint64_t timespec_to_nsec(const struct timespec *ts)
+{
+ return ts->tv_nsec + (ts->tv_sec * NSEC_PER_SEC);
+}
+
+void Util::perf_begin(perf_counter_t perf)
+{
+ struct perf_counter_elapsed_t *perf_elapsed = (struct perf_counter_elapsed_t *)perf;
+ 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;
+ }
+
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ perf_elapsed->start = timespec_to_nsec(&ts);
+}
+
+void Util::perf_end(perf_counter_t perf)
+{
+ struct perf_counter_elapsed_t *perf_elapsed = (struct perf_counter_elapsed_t *)perf;
+
+ 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);
+ return;
+ }
+
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ const uint64_t elapsed = timespec_to_nsec(&ts) - perf_elapsed->start;
+
+ perf_elapsed->count++;
+ perf_elapsed->total += elapsed;
+
+ if (perf_elapsed->least > elapsed) {
+ perf_elapsed->least = elapsed;
+ }
+
+ if (perf_elapsed->most < elapsed) {
+ perf_elapsed->most = elapsed;
+ }
+
+ /*
+ * Maintain mean and variance of interval in nanoseconds
+ * 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_count = (struct perf_counter_count_t *)perf;
+
+ if (perf_count->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_count->base.name);
+ return;
+ }
+
+ perf_count->count++;
+}
+
+#endif
diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h
index 36ee1507d5..59c72ed0ca 100644
--- a/libraries/AP_HAL_Linux/Util.h
+++ b/libraries/AP_HAL_Linux/Util.h
@@ -56,6 +56,11 @@ 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;
+
private:
static Linux::ToneAlarm _toneAlarm;
Linux::Heat *_heat;