2015-11-03 12:13:03 -04:00
|
|
|
/// -*- 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 <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
|
|
|
|
|
|
|
#include <limits.h>
|
|
|
|
#include <time.h>
|
|
|
|
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
|
|
|
|
#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;
|
2015-11-13 10:08:53 -04:00
|
|
|
|
|
|
|
if (perf_elapsed == NULL) {
|
|
|
|
return;
|
|
|
|
}
|
2015-11-03 12:13:03 -04:00
|
|
|
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;
|
|
|
|
|
2015-11-13 10:08:53 -04:00
|
|
|
if (perf_elapsed == NULL) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-11-03 12:13:03 -04:00
|
|
|
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)
|
|
|
|
{
|
2015-11-13 10:08:53 -04:00
|
|
|
struct perf_counter_count_t *perf_counter = (struct perf_counter_count_t *)perf;
|
|
|
|
|
|
|
|
if (perf_counter == NULL) {
|
|
|
|
return;
|
|
|
|
}
|
2015-11-03 12:13:03 -04:00
|
|
|
|
2015-11-13 10:08:53 -04:00
|
|
|
if (perf_counter->base.type != PC_COUNT) {
|
2015-11-03 12:13:03 -04:00
|
|
|
hal.console->printf("perf_count() called over a perf_counter_t(%s) "
|
|
|
|
"that is not of the PC_COUNT type.\n",
|
2015-11-13 10:08:53 -04:00
|
|
|
perf_counter->base.name);
|
2015-11-03 12:13:03 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-11-13 10:08:53 -04:00
|
|
|
perf_counter->count++;
|
2015-11-03 12:13:03 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|