Line data Source code
1 : /*
2 : Please contribute your ideas! See https://ardupilot.club/dev for details
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : /*
18 : Management for hal.storage to allow for backwards compatible mapping
19 : of storage offsets to available storage
20 : */
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <AP_Math/AP_Math.h>
24 :
25 : #include <AP_Vehicle/AP_Vehicle_Type.h>
26 : #include <AP_BoardConfig/AP_BoardConfig.h>
27 : #include <AP_Filesystem/AP_Filesystem.h>
28 : #include <GCS_MAVLink/GCS.h>
29 :
30 : #include "StorageManager.h"
31 :
32 : #include <stdio.h>
33 :
34 : extern const AP_HAL::HAL& hal;
35 :
36 : bool StorageManager::last_io_failed;
37 :
38 : /*
39 : the layouts below are carefully designed to ensure backwards
40 : compatibility with older firmwares
41 : */
42 :
43 : #if STORAGE_NUM_AREAS == 1
44 : /*
45 : layout for peripherals
46 : */
47 : const StorageManager::StorageArea StorageManager::layout[STORAGE_NUM_AREAS] = {
48 : { StorageParam, 0, HAL_STORAGE_SIZE}
49 : };
50 :
51 : #else
52 : /*
53 : layout for fixed wing and rovers
54 : On PX4v1 this gives 309 waypoints, 30 rally points and 52 fence points
55 : On Pixhawk this gives 724 waypoints, 50 rally points and 84 fence points
56 : */
57 : /*
58 : layout for copter.
59 : On PX4v1 this gives 303 waypoints, 26 rally points and 38 fence points
60 : On Pixhawk this gives 718 waypoints, 46 rally points and 70 fence points
61 : */
62 : const StorageManager::StorageArea StorageManager::layout[STORAGE_NUM_AREAS] = {
63 : #if !APM_BUILD_COPTER_OR_HELI
64 : { StorageParam, 0, 1280}, // 0x500 parameter bytes
65 : { StorageMission, 1280, 2506},
66 : { StorageRally, 3786, 150}, // 10 rally points
67 : { StorageFence, 3936, 160}, // 20 fence points
68 : #else
69 : { StorageParam, 0, 1536}, // 0x600 param bytes
70 : { StorageMission, 1536, 2422},
71 : { StorageRally, 3958, 90}, // 6 rally points
72 : { StorageFence, 4048, 48}, // 6 fence points
73 : #endif
74 : #if STORAGE_NUM_AREAS >= 10
75 : { StorageParam, 4096, 1280},
76 : { StorageRally, 5376, 300},
77 : { StorageFence, 5676, 256},
78 : { StorageMission, 5932, 2132},
79 : { StorageKeys, 8064, 64},
80 : { StorageBindInfo,8128, 56},
81 : #endif
82 : #if STORAGE_NUM_AREAS == 11
83 : // optimised for lots of parameters for 15k boards with OSD
84 : { StorageParam, 8192, 7168},
85 : #elif STORAGE_NUM_AREAS == 12
86 : // optimised for lots of parameters for 15k boards with OSD, plus room for CAN DNA
87 : { StorageParam, 8192, 6144},
88 : { StorageCANDNA, 14336, 1024},
89 : #endif
90 : #if STORAGE_NUM_AREAS >= 15
91 : { StorageParam, 8192, 1280},
92 : { StorageRally, 9472, 300},
93 : { StorageFence, 9772, 256},
94 : { StorageMission, 10028, 5204}, // leave 128 byte gap for expansion
95 : { StorageCANDNA, 15232, 1024},
96 : // 128 byte gap at end of first 16k
97 : #endif
98 : #if STORAGE_NUM_AREAS >= 18
99 : { StorageParam, 16384, 1280},
100 : { StorageMission, 17664, 9842},
101 : { StorageParamBak, 27506, 5262},
102 : #endif
103 : };
104 : #endif // STORAGE_NUM_AREAS == 1
105 :
106 : /*
107 : erase all storage
108 : */
109 295 : void StorageManager::erase(void)
110 : {
111 295 : if (!hal.storage->erase()) {
112 0 : ::printf("StorageManager: erase failed\n");
113 : }
114 295 : }
115 :
116 : /*
117 : constructor for StorageAccess
118 : */
119 10871 : StorageAccess::StorageAccess(StorageManager::StorageType _type) :
120 10871 : type(_type)
121 : {
122 : // calculate available bytes
123 10871 : total_size = 0;
124 : #if AP_SDCARD_STORAGE_ENABLED
125 10871 : file = nullptr;
126 : #endif
127 173936 : for (uint8_t i=0; i<STORAGE_NUM_AREAS; i++) {
128 163065 : const StorageManager::StorageArea &area = StorageManager::layout[i];
129 163065 : if (area.type == type) {
130 28217 : total_size += area.length;
131 : }
132 : }
133 10871 : }
134 :
135 : /*
136 : base read function. The src offset is within the bytes allocated
137 : for the storage type of this StorageAccess object
138 : */
139 75221236 : bool StorageAccess::read_block(void *data, uint16_t addr, size_t n) const
140 : {
141 75221236 : uint8_t *b = (uint8_t *)data;
142 :
143 : #if AP_SDCARD_STORAGE_ENABLED
144 75221236 : if (file != nullptr) {
145 : // using microSD data
146 699997 : if (addr > file->bufsize) {
147 0 : return false;
148 : }
149 699997 : const size_t n2 = MIN(n, file->bufsize - addr);
150 699997 : memcpy(b, &file->buffer[addr], n2);
151 699997 : return n == n2;
152 : }
153 : #endif
154 :
155 87235668 : for (uint8_t i=0; i<STORAGE_NUM_AREAS; i++) {
156 87235668 : const StorageManager::StorageArea &area = StorageManager::layout[i];
157 87235668 : uint16_t length = area.length;
158 87235668 : uint16_t offset = area.offset;
159 87235668 : if (area.type != type) {
160 12511252 : continue;
161 : }
162 74724416 : if (addr >= length) {
163 : // the data isn't in this area
164 203149 : addr -= length;
165 203149 : continue;
166 : }
167 74521267 : uint16_t count = n;
168 74521267 : if (count+addr > length) {
169 : // the data crosses a boundary between two areas
170 28 : count = length - addr;
171 : }
172 74521267 : hal.storage->read_block(b, addr+offset, count);
173 74521267 : n -= count;
174 :
175 74521267 : if (n == 0) {
176 74521239 : break;
177 : }
178 :
179 : // move pointer after written bytes
180 28 : b += count;
181 : // continue writing at the beginning of next valid area
182 28 : addr = 0;
183 : }
184 :
185 74521239 : return (n == 0);
186 : }
187 :
188 :
189 : /*
190 : base write function. The addr offset is within the bytes allocated
191 : for the storage type of this StorageAccess object
192 : */
193 154469 : bool StorageAccess::write_block(uint16_t addr, const void *data, size_t n) const
194 : {
195 154469 : const uint8_t *b = (const uint8_t *)data;
196 :
197 : #if AP_SDCARD_STORAGE_ENABLED
198 154469 : if (file != nullptr) {
199 14531 : if (addr > file->bufsize) {
200 0 : return false;
201 : }
202 : // using microSD data
203 14531 : WITH_SEMAPHORE(file->sem);
204 14531 : const size_t n2 = MIN(n, file->bufsize - addr);
205 14531 : memcpy(&file->buffer[addr], b, n2);
206 29114 : for (uint8_t i=addr/1024U; i<(addr+n2+1023U)/1024U; i++) {
207 14583 : file->dirty_mask |= (1ULL<<i);
208 : }
209 14531 : return n == n2;
210 14531 : }
211 : #endif
212 :
213 220795 : for (uint8_t i=0; i<STORAGE_NUM_AREAS; i++) {
214 220797 : const StorageManager::StorageArea &area = StorageManager::layout[i];
215 220797 : uint16_t length = area.length;
216 220797 : uint16_t offset = area.offset;
217 220797 : if (area.type != type) {
218 77036 : continue;
219 : }
220 143761 : if (addr >= length) {
221 : // the data isn't in this area
222 3813 : addr -= length;
223 3813 : continue;
224 : }
225 139948 : uint16_t count = n;
226 139948 : if (count+addr > length) {
227 : // the data crosses a boundary between two areas
228 8 : count = length - addr;
229 : }
230 139948 : hal.storage->write_block(addr+offset, b, count);
231 139948 : n -= count;
232 :
233 139948 : if (n == 0) {
234 139940 : break;
235 : }
236 :
237 : // move pointer after written bytes
238 8 : b += count;
239 : // continue writing at the beginning of next valid area
240 8 : addr = 0;
241 : }
242 :
243 139938 : return (n == 0);
244 : }
245 :
246 : /*
247 : read a byte
248 : */
249 137723 : uint8_t StorageAccess::read_byte(uint16_t loc) const
250 : {
251 : uint8_t v;
252 137723 : read_block(&v, loc, sizeof(v));
253 137723 : return v;
254 : }
255 :
256 : /*
257 : read 16 bit value
258 : */
259 98064 : uint16_t StorageAccess::read_uint16(uint16_t loc) const
260 : {
261 : uint16_t v;
262 98064 : read_block(&v, loc, sizeof(v));
263 98064 : return v;
264 : }
265 :
266 : /*
267 : read 32 bit value
268 : */
269 11059 : uint32_t StorageAccess::read_uint32(uint16_t loc) const
270 : {
271 : uint32_t v;
272 11059 : read_block(&v, loc, sizeof(v));
273 11059 : return v;
274 : }
275 :
276 : /*
277 : read a float
278 : */
279 76 : float StorageAccess::read_float(uint16_t loc) const
280 : {
281 : float v;
282 76 : read_block(&v, loc, sizeof(v));
283 76 : return v;
284 : }
285 :
286 : /*
287 : write a byte
288 : */
289 19299 : void StorageAccess::write_byte(uint16_t loc, uint8_t value) const
290 : {
291 19299 : write_block(loc, &value, sizeof(value));
292 19299 : }
293 :
294 : /*
295 : write a uint16
296 : */
297 15301 : void StorageAccess::write_uint16(uint16_t loc, uint16_t value) const
298 : {
299 15301 : write_block(loc, &value, sizeof(value));
300 15301 : }
301 :
302 : /*
303 : write a uint32
304 : */
305 4398 : void StorageAccess::write_uint32(uint16_t loc, uint32_t value) const
306 : {
307 4398 : write_block(loc, &value, sizeof(value));
308 4398 : }
309 :
310 : /*
311 : write a float
312 : */
313 22 : void StorageAccess::write_float(uint16_t loc, float value) const
314 : {
315 22 : write_block(loc, &value, sizeof(value));
316 22 : }
317 :
318 : /*
319 : copy one area to another
320 : */
321 0 : bool StorageAccess::copy_area(const StorageAccess &source) const
322 : {
323 : // we deliberately allow for copies from smaller areas. This
324 : // allows for a partial backup region for parameters
325 0 : uint16_t total = MIN(source.size(), size());
326 0 : uint16_t ofs = 0;
327 0 : while (total > 0) {
328 : uint8_t block[32];
329 0 : uint16_t n = MIN(sizeof(block), total);
330 0 : if (!source.read_block(block, ofs, n) ||
331 0 : !write_block(ofs, block, n)) {
332 0 : return false;
333 : }
334 0 : total -= n;
335 0 : ofs += n;
336 : }
337 0 : return true;
338 : }
339 :
340 : #if AP_SDCARD_STORAGE_ENABLED
341 : /*
342 : attach a file to a storage region
343 : */
344 3 : bool StorageAccess::attach_file(const char *filename, uint16_t size_kbyte)
345 : {
346 3 : if (file != nullptr) {
347 : // only one attach per boot
348 0 : return false;
349 : }
350 3 : const uint32_t size = MIN(0xFFFFU, size_kbyte * 1024U);
351 3 : auto *newfile = NEW_NOTHROW FileStorage;
352 3 : if (newfile == nullptr) {
353 0 : AP_BoardConfig::allocation_error("StorageFile");
354 : }
355 : ssize_t nread;
356 :
357 3 : newfile->fd = AP::FS().open(filename, O_RDWR | O_CREAT);
358 3 : if (newfile->fd == -1) {
359 0 : goto fail;
360 : }
361 3 : newfile->buffer = NEW_NOTHROW uint8_t[size];
362 3 : if (newfile->buffer == nullptr) {
363 0 : AP_BoardConfig::allocation_error("StorageFile");
364 : }
365 3 : newfile->bufsize = size;
366 3 : nread = AP::FS().read(newfile->fd, newfile->buffer, size);
367 3 : if (nread == -1) {
368 0 : goto fail;
369 : }
370 3 : if (nread == 0) {
371 : // new file, copy storage from existing to allow users to
372 : // start with existing mission
373 0 : read_block(newfile->buffer, 0, total_size);
374 : }
375 3 : if (nread < int32_t(size)) {
376 0 : if (AP::FS().write(newfile->fd, &newfile->buffer[nread], size-nread) != int32_t(size-nread)) {
377 0 : goto fail;
378 : }
379 0 : if (AP::FS().fsync(newfile->fd) != 0) {
380 0 : goto fail;
381 : }
382 : }
383 :
384 3 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&StorageAccess::flush_file, void));
385 :
386 3 : file = newfile;
387 3 : total_size = newfile->bufsize;
388 :
389 3 : return true;
390 :
391 0 : fail:
392 0 : if (newfile->fd != -1) {
393 0 : AP::FS().close(newfile->fd);
394 : }
395 0 : if (newfile->buffer != nullptr) {
396 0 : delete[] newfile->buffer;
397 : }
398 0 : delete newfile;
399 0 : return false;
400 : }
401 :
402 : /*
403 : flush file changes to microSD
404 : */
405 113894 : void StorageAccess::flush_file(void)
406 : {
407 113894 : if (file == nullptr || file->dirty_mask == 0) {
408 113821 : return;
409 : }
410 73 : const uint32_t now_ms = AP_HAL::millis();
411 73 : if (now_ms - file->last_clean_ms < 1000U) {
412 0 : return;
413 : }
414 73 : WITH_SEMAPHORE(file->sem);
415 :
416 73 : if (StorageManager::last_io_failed &&
417 0 : now_ms - file->last_io_fail_ms < 2000U) {
418 : // don't retry too fast
419 0 : return;
420 : }
421 :
422 : // write out 1k at a time
423 73 : bool io_fail = false;
424 73 : const int b = __builtin_ffsll(file->dirty_mask);
425 73 : const uint32_t ofs = (b-1)*1024;
426 73 : const uint32_t len = MIN(1024U, file->bufsize-ofs);
427 146 : if (AP::FS().lseek(file->fd, ofs, SEEK_SET) != int32_t(ofs) ||
428 73 : AP::FS().write(file->fd, &file->buffer[ofs], len) != int32_t(len)) {
429 0 : io_fail = true;
430 : } else {
431 73 : file->dirty_mask &= ~(1ULL<<(b-1));
432 : }
433 73 : if (file->dirty_mask == 0) {
434 5 : file->last_clean_ms = now_ms;
435 5 : if (AP::FS().fsync(file->fd) != 0) {
436 0 : io_fail = true;
437 : }
438 : }
439 73 : if (io_fail && !StorageManager::last_io_failed) {
440 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mission storage failed");
441 73 : } else if (!io_fail && StorageManager::last_io_failed) {
442 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mission storage OK");
443 : }
444 73 : StorageManager::last_io_failed = io_fail;
445 73 : if (io_fail) {
446 0 : file->last_io_fail_ms = now_ms;
447 : }
448 73 : }
449 : #endif // AP_SDCARD_STORAGE_ENABLED
450 :
|
