mirror of https://github.com/ArduPilot/ardupilot
88 lines
2.5 KiB
C++
88 lines
2.5 KiB
C++
|
/*
|
||
|
|
* 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/>.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
#ifdef ENABLE_PROFILE
|
||
|
|
#include <AP_HAL/AP_HAL.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <errno.h>
|
||
|
|
|
||
|
|
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
|
||
|
|
|
||
|
|
struct FI {
|
||
|
|
uint32_t address;
|
||
|
|
uint32_t count;
|
||
|
|
struct FI * next;
|
||
|
|
};
|
||
|
|
|
||
|
|
FI* ht[1024];
|
||
|
|
|
||
|
|
extern "C" __attribute__((section(".iram1"))) __attribute__((no_instrument_function))
|
||
|
|
void _mcount()
|
||
|
|
{
|
||
|
|
void *a = __builtin_return_address(0);
|
||
|
|
a = __builtin_extract_return_addr(a);
|
||
|
|
uint32_t address = (uint32_t) a;
|
||
|
|
uint32_t hash = address;
|
||
|
|
hash = ((hash >> 16)^hash)*0x45d9f3b;
|
||
|
|
hash = ((hash >> 16)^hash)*0x45d9f3b;
|
||
|
|
uint32_t index = ((hash >> 16)^hash)%ARRAY_SIZE(ht);
|
||
|
|
FI **current = &(ht[index]);
|
||
|
|
while (*current != nullptr && (*current)->address != address) {
|
||
|
|
current = &((*current)->next);
|
||
|
|
}
|
||
|
|
if (*current != nullptr) {
|
||
|
|
(*current)->count++;
|
||
|
|
} else {
|
||
|
|
FI* next = (FI *)calloc(1, sizeof(FI));
|
||
|
|
next->address = address;
|
||
|
|
next->count = 1;
|
||
|
|
next->next = nullptr;
|
||
|
|
*current = next;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void print_profile()
|
||
|
|
{
|
||
|
|
static int64_t last_run = 0;
|
||
|
|
static int counter = 0;
|
||
|
|
if (AP_HAL::millis64() - last_run > 60000) {
|
||
|
|
//char fname[50];
|
||
|
|
//snprintf(fname, sizeof(fname), "/SDCARD/APM/PROF%03d.TXT", counter);
|
||
|
|
++counter;
|
||
|
|
//FILE *f = fopen(fname, "w");
|
||
|
|
// if (f != nullptr) {
|
||
|
|
for (size_t i=0; i < ARRAY_SIZE(ht); i++) {
|
||
|
|
for (FI *current = ht[i]; current != nullptr; current = current->next) {
|
||
|
|
if (current->count != 0) {
|
||
|
|
printf("0x%016x 0x%x\n", current->address, current->count);
|
||
|
|
current->count = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// fclose(f);
|
||
|
|
printf("------- profile dumped %d\n", counter);
|
||
|
|
/*} else {
|
||
|
|
printf("profile open error %d %d\n", counter, errno);
|
||
|
|
}*/
|
||
|
|
last_run = AP_HAL::millis64();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#else
|
||
|
|
void print_profile()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|