Line data Source code
1 : #include <stdarg.h>
2 : #include <errno.h>
3 : #include <limits.h>
4 : #include <stdio.h>
5 : #include <sys/stat.h>
6 : #include <sys/types.h>
7 : #include <unistd.h>
8 : #include <fcntl.h>
9 :
10 : #include <AP_HAL/AP_HAL.h>
11 : #include <AP_HAL/system.h>
12 :
13 : #include "Scheduler.h"
14 : #include <AP_Math/div1000.h>
15 :
16 : extern const AP_HAL::HAL& hal;
17 :
18 : using HALSITL::Scheduler;
19 :
20 : namespace AP_HAL {
21 :
22 : static struct {
23 : uint64_t start_time_ns;
24 : } state;
25 :
26 2247 : static uint64_t ts_to_nsec(struct timespec &ts)
27 : {
28 2247 : return ts.tv_sec*1000000000ULL + ts.tv_nsec;
29 : }
30 :
31 2145 : void init()
32 : {
33 2145 : struct timespec ts {};
34 2145 : clock_gettime(CLOCK_MONOTONIC, &ts);
35 2145 : state.start_time_ns = ts_to_nsec(ts);
36 2145 : }
37 :
38 0 : void WEAK panic(const char *errormsg, ...)
39 : {
40 : va_list ap;
41 :
42 0 : fflush(stdout);
43 0 : printf("PANIC: ");
44 0 : va_start(ap, errormsg);
45 0 : vprintf(errormsg, ap);
46 0 : va_end(ap);
47 0 : printf("\n");
48 :
49 0 : dump_stack_trace();
50 0 : dump_core_file();
51 :
52 0 : if (getenv("SITL_PANIC_EXIT")) {
53 : // this is used on the autotest server to prevent us waiting
54 : // 10 hours for a timeout
55 0 : printf("panic and SITL_PANIC_EXIT set - exitting");
56 0 : exit(1);
57 : }
58 0 : for(;;);
59 : }
60 :
61 : // partly flogged from: https://github.com/tridge/junkcode/blob/master/segv_handler/segv_handler.c
62 0 : static void run_command_on_ownpid(const char *commandname)
63 : {
64 : // find dumpstack command:
65 0 : const char *command_filepath = commandname; // if we can't find it trust in PATH
66 : struct stat statbuf;
67 0 : const char *custom_scripts_dir_path = getenv("AP_SCRIPTS_DIR_PATH");
68 0 : char *custom_scripts_dir_path_pattern = nullptr;
69 0 : if (custom_scripts_dir_path != nullptr) {
70 0 : if (asprintf(&custom_scripts_dir_path_pattern, "%s/%%s", custom_scripts_dir_path) == -1) {
71 0 : custom_scripts_dir_path_pattern = nullptr;
72 : }
73 : }
74 0 : const char *paths[] {
75 : custom_scripts_dir_path_pattern,
76 : "Tools/scripts/%s",
77 : "APM/Tools/scripts/%s", // for autotest server
78 : "../Tools/scripts/%s", // when run from e.g. ArduCopter subdirectory
79 0 : };
80 : // long enough for an absolute path from AP_SCRIPTS_DIR_PATH; the 60
81 : // bytes this used to be was not - a checkout under a path of any
82 : // length silently truncated, and the truncated name simply failed to
83 : // stat, so the override looked as though it had been ignored
84 : char buffer[PATH_MAX];
85 0 : for (uint8_t i=0; i<ARRAY_SIZE(paths); i++) {
86 0 : if (paths[i] == nullptr) {
87 0 : continue;
88 : }
89 : // form up a filepath from each path and commandname; if it
90 : // exists, use it
91 0 : const int len = snprintf(buffer, sizeof(buffer), paths[i], commandname);
92 0 : if (len < 0 || (unsigned)len >= sizeof(buffer)) {
93 : // truncated, so this is not the path we were asked for
94 0 : continue;
95 : }
96 0 : if (::stat(buffer, &statbuf) != -1) {
97 0 : command_filepath = buffer;
98 0 : break;
99 : }
100 : }
101 0 : free(custom_scripts_dir_path_pattern);
102 :
103 : char progname[100];
104 0 : int n = readlink("/proc/self/exe", progname, sizeof(progname)-1);
105 0 : if (n == -1) {
106 0 : strncpy(progname, "unknown", sizeof(progname));
107 0 : n = strlen(progname);
108 : }
109 0 : progname[n] = 0;
110 :
111 0 : char *p = strrchr(progname, '/');
112 0 : if (p != nullptr) {
113 0 : *p = 0;
114 : } else {
115 0 : p = progname;
116 : }
117 :
118 : char output_filepath[80];
119 0 : snprintf(output_filepath,
120 : ARRAY_SIZE(output_filepath),
121 : "%s_%s.%d.out",
122 : commandname,
123 : p+1,
124 0 : (int)getpid());
125 : // must fit the command filepath found above, which may now be
126 : // an absolute path, plus the output filepath
127 : char cmd[PATH_MAX + sizeof(output_filepath) + 32];
128 0 : snprintf(cmd,
129 : sizeof(cmd),
130 : "sh %s %d >%s 2>&1",
131 : command_filepath,
132 0 : (int)getpid(),
133 : output_filepath);
134 0 : fprintf(stderr, "Running: %s\n", cmd);
135 :
136 0 : if (system(cmd)) {
137 0 : fprintf(stderr, "Failed\n");
138 0 : return;
139 : }
140 0 : fprintf(stderr, "%s has been run. Output was:\n", commandname);
141 0 : fprintf(stderr, "-------------- begin %s output ----------------\n", commandname);
142 : // print the trace on stderr:
143 0 : int fd = open(output_filepath, O_RDONLY);
144 0 : if (fd == -1) {
145 0 : fprintf(stderr, "Failed to open stack dump filepath: %m");
146 0 : return;
147 : }
148 0 : fflush(stderr); // we are about to write(2) around it
149 : char buf[1024]; // let's hope we're not here because we ran out of stack
150 : while (true) {
151 0 : const ssize_t ret = read(fd, buf, ARRAY_SIZE(buf));
152 0 : if (ret == -1) {
153 0 : if (errno == EINTR) {
154 : // one of our timers, not a real error
155 0 : continue;
156 : }
157 0 : fprintf(stderr, "Read error: %m");
158 0 : break;
159 : }
160 0 : if (ret == 0) {
161 0 : break;
162 : }
163 : // write() is allowed to write less than it was asked to - stderr
164 : // is a pipe when run from autotest, and a signal can cut a write
165 : // short - so keep going until the whole block is out. Treating a
166 : // short write as fatal silently truncated the output, which is
167 : // the one thing we came here to collect.
168 0 : ssize_t written = 0;
169 0 : while (written < ret) {
170 0 : const ssize_t wrote = write(2, &buf[written], ret - written);
171 0 : if (wrote == -1) {
172 0 : if (errno == EINTR) {
173 0 : continue;
174 : }
175 0 : break;
176 : }
177 0 : written += wrote;
178 : }
179 0 : if (written != ret) {
180 0 : fprintf(stderr, "Write error after %d of %d bytes: %m\n",
181 : (int)written, (int)ret);
182 0 : break;
183 : }
184 0 : }
185 0 : fprintf(stderr, "-------------- end %s output ----------------\n", commandname);
186 0 : close(fd);
187 : }
188 0 : void dump_stack_trace()
189 : {
190 0 : run_command_on_ownpid("dumpstack.sh");
191 0 : }
192 0 : void dump_core_file()
193 : {
194 0 : run_command_on_ownpid("dumpcore.sh");
195 0 : }
196 :
197 4142240127 : uint32_t micros()
198 : {
199 4142240127 : return micros64() & 0xFFFFFFFF;
200 : }
201 :
202 13228532818 : uint32_t millis()
203 : {
204 13228532818 : return millis64() & 0xFFFFFFFF;
205 : }
206 :
207 21998890816 : uint64_t micros64()
208 : {
209 21998890816 : const HALSITL::Scheduler* scheduler = HALSITL::Scheduler::from(hal.scheduler);
210 21995615970 : uint64_t stopped_usec = scheduler->stopped_clock_usec();
211 21993635985 : if (stopped_usec) {
212 21993635883 : return stopped_usec;
213 : }
214 :
215 : struct timespec ts;
216 102 : clock_gettime(CLOCK_MONOTONIC, &ts);
217 102 : return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns);
218 : }
219 :
220 13357930611 : uint64_t millis64()
221 : {
222 13357930611 : return uint64_div1000(micros64());
223 : }
224 :
225 : } // namespace AP_HAL
|
