Line data Source code
1 : #include <errno.h>
2 : #include <fcntl.h>
3 : #include <stdarg.h>
4 : #include <stdio.h>
5 : #include <stdlib.h>
6 : #include <sys/stat.h>
7 : #include <time.h>
8 : #include <unistd.h>
9 :
10 : #include <AP_HAL/AP_HAL.h>
11 :
12 : #include "Heat_Pwm.h"
13 : #include "Util.h"
14 :
15 : using namespace Linux;
16 :
17 : extern const AP_HAL::HAL& hal;
18 :
19 : #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
20 : ToneAlarm_Disco Util::_toneAlarm;
21 : #else
22 : ToneAlarm Util::_toneAlarm;
23 : #endif
24 :
25 0 : void Util::init(int argc, char * const *argv) {
26 0 : saved_argc = argc;
27 0 : saved_argv = argv;
28 :
29 : #ifdef HAL_UTILS_HEAT
30 : #if HAL_UTILS_HEAT == HAL_LINUX_HEAT_PWM
31 : _heat = NEW_NOTHROW Linux::HeatPwm(HAL_LINUX_HEAT_PWM_NUM,
32 : HAL_LINUX_HEAT_KP,
33 : HAL_LINUX_HEAT_KI,
34 : HAL_LINUX_HEAT_PERIOD_NS);
35 : #else
36 : #error Unrecognized Heat
37 : #endif // #if
38 : #else
39 0 : _heat = NEW_NOTHROW Linux::Heat();
40 : #endif // #ifdef
41 0 : }
42 :
43 : // set current IMU temperatue in degrees C
44 0 : void Util::set_imu_temp(float current)
45 : {
46 0 : _heat->set_imu_temp(current);
47 0 : }
48 :
49 : // set target IMU temperatue in degrees C
50 0 : void Util::set_imu_target_temp(int8_t *target)
51 : {
52 0 : _heat->set_imu_target_temp(target);
53 0 : }
54 :
55 : /**
56 : return commandline arguments, if available
57 : */
58 0 : void Util::commandline_arguments(uint8_t &argc, char * const *&argv)
59 : {
60 0 : argc = saved_argc;
61 0 : argv = saved_argv;
62 0 : }
63 :
64 0 : uint64_t Util::get_hw_rtc() const
65 : {
66 : struct timespec ts;
67 0 : clock_gettime(CLOCK_REALTIME, &ts);
68 0 : const uint64_t seconds = ts.tv_sec;
69 0 : const uint64_t nanoseconds = ts.tv_nsec;
70 0 : return (seconds * 1000000ULL + nanoseconds/1000ULL);
71 : }
72 :
73 : #ifndef AP_HAL_LINUX_SET_HW_RTC_ENABLED
74 : #define AP_HAL_LINUX_SET_HW_RTC_ENABLED 1
75 : #endif // AP_HAL_LINUX_SET_HW_RTC_ENABLED
76 :
77 20 : void Util::set_hw_rtc(uint64_t time_utc_usec)
78 : {
79 : #if AP_HAL_LINUX_SET_HW_RTC_ENABLED
80 : timespec ts;
81 : ts.tv_sec = time_utc_usec/1000000ULL;
82 : ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL;
83 : clock_settime(CLOCK_REALTIME, &ts);
84 : #endif // AP_HAL_LINUX_SET_HW_RTC_ENABLED
85 20 : }
86 :
87 0 : bool Util::is_chardev_node(const char *path)
88 : {
89 : struct stat st;
90 :
91 0 : if (!path || lstat(path, &st) < 0) {
92 0 : return false;
93 : }
94 :
95 0 : return S_ISCHR(st.st_mode);
96 : }
97 :
98 : /*
99 : always report 256k of free memory. Using mallinfo() isn't useful as
100 : it only reported the current heap, which auto-expands. What we're
101 : trying to do here is ensure that code which checks for free memory
102 : before allocating objects does allow the allocation
103 : */
104 0 : uint32_t Util::available_memory(void)
105 : {
106 0 : return 256*1024;
107 : }
108 :
109 : #ifndef HAL_LINUX_DEFAULT_SYSTEM_ID
110 : #define HAL_LINUX_DEFAULT_SYSTEM_ID "linux-unknown"
111 : #endif
112 :
113 : /*
114 : get a (hopefully unique) machine ID
115 : */
116 0 : bool Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len)
117 : {
118 0 : char *cbuf = (char *)buf;
119 :
120 : // try first to use machine-id file. Most systems will have this
121 0 : const char *paths[] = { "/etc/machine-id", "/var/lib/dbus/machine-id" };
122 0 : for (uint8_t i=0; i<ARRAY_SIZE(paths); i++) {
123 0 : int fd = open(paths[i], O_RDONLY);
124 0 : if (fd == -1) {
125 0 : continue;
126 : }
127 0 : ssize_t ret = read(fd, buf, len);
128 0 : close(fd);
129 0 : if (ret <= 0) {
130 0 : continue;
131 : }
132 0 : len = ret;
133 0 : char *p = strchr(cbuf, '\n');
134 0 : if (p) {
135 0 : *p = 0;
136 : }
137 0 : len = strnlen(cbuf, len);
138 0 : return true;
139 : }
140 :
141 : // fallback to hostname
142 0 : if (gethostname(cbuf, len) != 0) {
143 : // use a default name so this always succeeds. Without it we can't
144 : // implement some features (such as UAVCAN)
145 0 : strncpy(cbuf, HAL_LINUX_DEFAULT_SYSTEM_ID, len);
146 : }
147 0 : len = strnlen(cbuf, len);
148 0 : return true;
149 : }
150 :
151 : /*
152 : as get_system_id_unformatted will already be ascii, we use the same
153 : ID here
154 : */
155 0 : bool Util::get_system_id(char buf[50])
156 : {
157 0 : uint8_t len = 40;
158 0 : return get_system_id_unformatted((uint8_t *)buf, len);
159 : }
160 :
161 :
162 0 : int Util::write_file(const char *path, const char *fmt, ...)
163 : {
164 0 : errno = 0;
165 :
166 0 : int fd = open(path, O_WRONLY | O_CLOEXEC);
167 0 : if (fd == -1) {
168 0 : return -errno;
169 : }
170 :
171 : va_list args;
172 0 : va_start(args, fmt);
173 :
174 0 : int ret = vdprintf(fd, fmt, args);
175 0 : int errno_bkp = errno;
176 0 : close(fd);
177 :
178 0 : va_end(args);
179 :
180 0 : if (ret < 1) {
181 0 : return -errno_bkp;
182 : }
183 :
184 0 : return ret;
185 : }
186 :
187 0 : int Util::read_file(const char *path, const char *fmt, ...)
188 : {
189 0 : errno = 0;
190 :
191 0 : FILE *file = fopen(path, "re");
192 0 : if (!file) {
193 0 : return -errno;
194 : }
195 :
196 : va_list args;
197 0 : va_start(args, fmt);
198 :
199 0 : int ret = vfscanf(file, fmt, args);
200 0 : int errno_bkp = errno;
201 0 : fclose(file);
202 :
203 0 : va_end(args);
204 :
205 0 : if (ret < 1) {
206 0 : return -errno_bkp;
207 : }
208 :
209 0 : return ret;
210 : }
211 :
212 : const char *Linux::Util::_hw_names[UTIL_NUM_HARDWARES] = {
213 : [UTIL_HARDWARE_RPI1] = "BCM2708",
214 : [UTIL_HARDWARE_RPI2] = "BCM2709",
215 : [UTIL_HARDWARE_RPI4] = "BCM2711",
216 : [UTIL_HARDWARE_BEBOP] = "Mykonos3 board",
217 : [UTIL_HARDWARE_BEBOP2] = "Milos board",
218 : [UTIL_HARDWARE_DISCO] = "Evinrude board",
219 : };
220 :
221 : #define MAX_SIZE_LINE 50
222 0 : int Util::get_hw_arm32()
223 : {
224 0 : char buffer[MAX_SIZE_LINE] = { 0 };
225 0 : FILE *f = fopen("/proc/cpuinfo", "r");
226 0 : if (f == nullptr) {
227 0 : return -errno;
228 : }
229 :
230 0 : while (fgets(buffer, MAX_SIZE_LINE, f) != nullptr) {
231 0 : if (strstr(buffer, "Hardware") == nullptr) {
232 0 : continue;
233 : }
234 0 : for (uint8_t i = 0; i < UTIL_NUM_HARDWARES; i++) {
235 0 : if (strstr(buffer, _hw_names[i]) == nullptr) {
236 0 : continue;
237 : }
238 0 : fclose(f);
239 0 : return i;
240 : }
241 : }
242 :
243 0 : fclose(f);
244 0 : return -ENOENT;
245 : }
246 :
247 : /**
248 : * This method will read random values with set size.
249 : */
250 1000000 : bool Util::get_random_vals(uint8_t* data, size_t size)
251 : {
252 1000000 : int dev_random = open("/dev/urandom", O_RDONLY);
253 1000000 : if (dev_random < 0) {
254 0 : return false;
255 : }
256 1000000 : ssize_t result = read(dev_random, data, size);
257 1000000 : close(dev_random);
258 1000000 : return result >= 0 && (size_t)result == size;
259 : }
260 :
261 0 : bool Util::parse_cpu_set(const char *str, cpu_set_t *cpu_set) const
262 : {
263 : unsigned long cpu1, cpu2;
264 : char *endptr, sep;
265 :
266 0 : CPU_ZERO(cpu_set);
267 :
268 : do {
269 0 : cpu1 = strtoul(str, &endptr, 10);
270 0 : if (str == endptr) {
271 0 : return false;
272 : }
273 :
274 0 : str = endptr + 1;
275 0 : sep = *endptr;
276 0 : if (sep == ',' || sep == '\0') {
277 0 : CPU_SET(cpu1, cpu_set);
278 0 : continue;
279 0 : }
280 :
281 0 : if (sep != '-') {
282 0 : return false;
283 : }
284 :
285 0 : cpu2 = strtoul(str, &endptr, 10);
286 0 : if (str == endptr) {
287 0 : return false;
288 : }
289 :
290 0 : str = endptr + 1;
291 0 : for (; cpu1 <= cpu2; cpu1++) {
292 0 : CPU_SET(cpu1, cpu_set);
293 : }
294 0 : } while (*endptr != '\0');
295 :
296 0 : return true;
297 : }
|
