diff --git a/libraries/AP_HAL_Linux/Perf.cpp b/libraries/AP_HAL_Linux/Perf.cpp deleted file mode 100644 index 12c39d5aa9..0000000000 --- a/libraries/AP_HAL_Linux/Perf.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/* - * 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 . - */ -#include -#include -#include -#include - -#include -#include - -#include "AP_HAL_Linux.h" -#include "Util.h" -#include "Perf.h" -#include "Perf_Lttng.h" - -#ifndef PRIu64 -#define PRIu64 "llu" -#endif - -using namespace Linux; - -extern const AP_HAL::HAL& hal; - -Perf *Perf::_singleton; - -static inline uint64_t now_nsec() -{ - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return ts.tv_nsec + (ts.tv_sec * AP_NSEC_PER_SEC); -} - -Perf *Perf::get_singleton() -{ - if (!_singleton) { - _singleton = new Perf(); - } - - return _singleton; -} - -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: %" PRIu64 "\t" - "min: %" PRIu64 "\t" - "max: %" PRIu64 "\t" - "avg: %.4f\t" - "stddev: %.4f\n", - c.name, c.count, c.min, c.max, c.avg, sqrt(c.m2)); - } else { - fprintf(stderr, "%-30s\t" - "count: %" PRIu64 "\n", - c.name, c.count); - } - } - - _last_debug_msec = now; -} - -Perf::Perf() -{ - if (pthread_rwlock_init(&_perf_counters_lock, nullptr) != 0) { - AP_HAL::panic("Perf: fail to initialize rw lock"); - } - - /* 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); - -#ifdef DEBUG_PERF - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&Perf::_debug_counters, void)); -#endif -} - -void Perf::begin(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 twice on perf_counter_t(%s)\n", - perf.name); - return; - } - - _update_count++; - - perf.start = now_nsec(); - - perf.lttng.begin(perf.name); -} - -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.min > elapsed) { - perf.min = elapsed; - } - - if (perf.max < elapsed) { - perf.max = elapsed; - } - - /* - * Maintain avg and variance of interval in nanoseconds - * Knuth/Welford recursive avg and variance of update intervals (via Wikipedia) - * Same implementation of PX4. - */ - const double delta_intvl = elapsed - perf.avg; - perf.avg += (delta_intvl / perf.count); - perf.m2 += (delta_intvl * (elapsed - perf.avg)); - perf.start = 0; - - perf.lttng.end(perf.name); -} - -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++; - - perf.lttng.count(perf.name, perf.count); -} - -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; -} diff --git a/libraries/AP_HAL_Linux/Perf.h b/libraries/AP_HAL_Linux/Perf.h deleted file mode 100644 index 5f74332769..0000000000 --- a/libraries/AP_HAL_Linux/Perf.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * 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 . - */ -#pragma once - -#include -#include -#include -#include - -#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_} - , min{ULONG_MAX} - { - } - - const char *name; - Perf_Lttng lttng; - - perf_counter_type type; - - uint64_t count; - - /* Everything below is in nanoseconds */ - uint64_t start; - uint64_t total; - uint64_t min; - uint64_t max; - - double avg; - 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_singleton(); - - 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 *_singleton; - - Perf(); - - void _debug_counters(); - - uint64_t _last_debug_msec; - - std::vector _perf_counters; - - /* synchronize addition of new perf counters */ - pthread_rwlock_t _perf_counters_lock; - - /* allow to check if memory pool has changed */ - std::atomic _update_count; -}; - -} diff --git a/libraries/AP_HAL_Linux/Perf_Lttng.cpp b/libraries/AP_HAL_Linux/Perf_Lttng.cpp deleted file mode 100644 index fb3819c5f1..0000000000 --- a/libraries/AP_HAL_Linux/Perf_Lttng.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* - This program 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 program 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 . - */ -#ifdef HAVE_LTTNG_UST - -#define TRACEPOINT_CREATE_PROBES -#define TRACEPOINT_DEFINE - -#include - -#include "AP_HAL_Linux.h" -#include "Perf_Lttng_TracePoints.h" -#include "Perf_Lttng.h" - -using namespace Linux; - -void Perf_Lttng::begin(const char *name) -{ - tracepoint(ardupilot, begin, name); -} - -void Perf_Lttng::end(const char *name) -{ - tracepoint(ardupilot, end, name); -} - -void Perf_Lttng::count(const char *name, uint64_t val) -{ - tracepoint(ardupilot, count, name, val); -} - -#else - -#include "Perf_Lttng.h" - -using namespace Linux; - -void Perf_Lttng::begin(const char *name) { } -void Perf_Lttng::end(const char *name) { } -void Perf_Lttng::count(const char *name, uint64_t val) { } - -#endif diff --git a/libraries/AP_HAL_Linux/Perf_Lttng.h b/libraries/AP_HAL_Linux/Perf_Lttng.h deleted file mode 100644 index daa48b5673..0000000000 --- a/libraries/AP_HAL_Linux/Perf_Lttng.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - This program 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 program 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 . - */ -#pragma once - -#include - -#include "AP_HAL_Linux.h" - -namespace Linux { - -class Perf_Lttng { -public: - void begin(const char *name); - void end(const char *name); - void count(const char *name, uint64_t val); -}; - -} diff --git a/libraries/AP_HAL_Linux/Perf_Lttng_TracePoints.h b/libraries/AP_HAL_Linux/Perf_Lttng_TracePoints.h deleted file mode 100644 index ae70d93d38..0000000000 --- a/libraries/AP_HAL_Linux/Perf_Lttng_TracePoints.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - This program 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 program 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 . - */ - -#undef TRACEPOINT_PROVIDER -#define TRACEPOINT_PROVIDER ardupilot - -#undef TRACEPOINT_INCLUDE -#define TRACEPOINT_INCLUDE - -#if !defined(_PERF_LTTNG_TRACEPOINT_H) || defined(TRACEPOINT_HEADER_MULTI_READ) -#define _PERF_LTTNG_TRACEPOINT_H - -#include - -TRACEPOINT_EVENT( - ardupilot, - begin, - TP_ARGS( - const char*, name_arg - ), - TP_FIELDS( - ctf_string(name_field, name_arg) - ) -) - -TRACEPOINT_EVENT( - ardupilot, - end, - TP_ARGS( - const char*, name_arg - ), - TP_FIELDS( - ctf_string(name_field, name_arg) - ) -) - -TRACEPOINT_EVENT( - ardupilot, - count, - TP_ARGS( - const char*, name_arg, - int, count_arg - ), - TP_FIELDS( - ctf_string(name_field, name_arg) - ctf_integer(int, count_field, count_arg) - ) -) - -#endif - -#include diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index e664cf71f8..01cbd9fc52 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -4,7 +4,6 @@ #include #include "Heat.h" -#include "Perf.h" #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO #include "ToneAlarm_Disco.h" #endif @@ -88,26 +87,6 @@ public: */ int read_file(const char *path, const char *fmt, ...) FMT_SCANF(3, 4); - perf_counter_t perf_alloc(enum perf_counter_type t, const char *name) override - { - return Perf::get_singleton()->add(t, name); - } - - void perf_begin(perf_counter_t perf) override - { - return Perf::get_singleton()->begin(perf); - } - - void perf_end(perf_counter_t perf) override - { - return Perf::get_singleton()->end(perf); - } - - void perf_count(perf_counter_t perf) override - { - return Perf::get_singleton()->count(perf); - } - int get_hw_arm32(); bool toneAlarm_init(uint8_t types) override { return _toneAlarm.init(); }