Line data Source code
1 : #include "Storage.h"
2 :
3 : #include <AP_Vehicle/AP_Vehicle_Type.h>
4 : #include <AP_HAL/AP_HAL.h>
5 : #include "AP_HAL_SITL.h"
6 :
7 : #include <assert.h>
8 : #include <sys/types.h>
9 : #include <sys/stat.h>
10 : #include <fcntl.h>
11 : #include <unistd.h>
12 : #include <stdio.h>
13 :
14 : #ifndef HAL_STORAGE_FILE
15 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
16 : #define HAL_STORAGE_FILE "eeprom-replay.bin"
17 : #elif APM_BUILD_TYPE(APM_BUILD_AP_Periph)
18 : #define HAL_STORAGE_FILE "eeprom-periph.bin"
19 : #else
20 : #define HAL_STORAGE_FILE "eeprom.bin"
21 : #endif
22 : #endif
23 :
24 : using namespace HALSITL;
25 :
26 : extern HAL_SITL& hal;
27 :
28 : /*
29 : emulate flash sector sizes
30 : */
31 : #ifndef HAL_FLASH_SECTOR_SIZE
32 : #if HAL_STORAGE_SIZE <= 16384
33 : #define HAL_FLASH_SECTOR_SIZE (16*1024)
34 : #elif HAL_STORAGE_SIZE <= 32768
35 : #define HAL_FLASH_SECTOR_SIZE (32*1024)
36 : #else
37 : #define HAL_FLASH_SECTOR_SIZE (128*1024)
38 : #endif
39 : #endif
40 :
41 : #ifndef HAL_FLASH_MIN_WRITE_SIZE
42 : #define HAL_FLASH_MIN_WRITE_SIZE 1
43 : #endif
44 :
45 : #ifndef HAL_FLASH_ALLOW_UPDATE
46 : #define HAL_FLASH_ALLOW_UPDATE 1
47 : #endif
48 :
49 74614143 : void Storage::_storage_open(void)
50 : {
51 74614143 : if (_initialisedType != StorageBackend::None) {
52 : // don't reinit
53 74612047 : return;
54 : }
55 :
56 2096 : _dirty_mask.clearall();
57 :
58 : #define HAL_RAMTRON_ALLOW_FALLBACK 0
59 :
60 : #if STORAGE_USE_FRAM
61 2096 : if (hal.get_storage_fram_enabled()) {
62 3 : if (fram.init() && fram.read(0, _buffer, HAL_STORAGE_SIZE)) {
63 : // _save_backup(); // FIXME
64 3 : _initialisedType = StorageBackend::FRAM;
65 3 : return;
66 : }
67 :
68 : #if !HAL_RAMTRON_ALLOW_FALLBACK
69 0 : AP_HAL::panic("Unable to init RAMTRON storage");
70 : #endif
71 : }
72 : #endif
73 :
74 : #if STORAGE_USE_FLASH
75 2093 : if (hal.get_storage_flash_enabled()) {
76 : // load from storage backend
77 3 : _flash_load();
78 3 : _initialisedType = StorageBackend::Flash;
79 3 : return;
80 : }
81 : #endif // STORAGE_USE_FLASH
82 :
83 : #if STORAGE_USE_POSIX
84 2090 : if (hal.get_storage_posix_enabled()) {
85 : // if we have failed filesystem init don't try again (this is
86 : // initialised to zero in the constructor)
87 2090 : if (log_fd == -1) {
88 0 : return;
89 : }
90 :
91 2090 : log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT, 0644);
92 2090 : if (log_fd == -1) {
93 0 : hal.console->printf("open failed of " HAL_STORAGE_FILE "\n");
94 0 : return;
95 : }
96 :
97 2090 : fcntl(log_fd, F_SETFD, FD_CLOEXEC);
98 :
99 2090 : int ret = read(log_fd, _buffer, HAL_STORAGE_SIZE);
100 2090 : if (ret < 0) {
101 0 : hal.console->printf("read failed for " HAL_STORAGE_FILE "\n");
102 0 : close(log_fd);
103 0 : log_fd = -1;
104 0 : return;
105 : }
106 : // pre-fill to full size
107 4180 : if (lseek(log_fd, ret, SEEK_SET) != ret ||
108 2090 : write(log_fd, &_buffer[ret], HAL_STORAGE_SIZE-ret) != HAL_STORAGE_SIZE-ret) {
109 0 : hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n");
110 0 : close(log_fd);
111 0 : log_fd = -1;
112 0 : return;
113 : }
114 2090 : _initialisedType = StorageBackend::SDCard; // AKA POSIX
115 2090 : return;
116 : }
117 : #endif
118 :
119 : // ::printf("No storage backend enabled");
120 : }
121 :
122 : /*
123 : mark some lines as dirty. Note that there is no attempt to avoid
124 : the race condition between this code and the _timer_tick() code
125 : below, which both update _dirty_mask. If we lose the race then the
126 : result is that a line is written more than once, but it won't result
127 : in a line not being written.
128 : */
129 92920 : void Storage::_mark_dirty(uint16_t loc, uint16_t length)
130 : {
131 92920 : if (length == 0) {
132 0 : return;
133 : }
134 92920 : uint16_t end = loc + length - 1;
135 230471 : for (uint16_t line=loc>>STORAGE_LINE_SHIFT;
136 230471 : line <= end>>STORAGE_LINE_SHIFT;
137 : line++) {
138 137551 : _dirty_mask.set(line);
139 : }
140 : }
141 :
142 74521223 : void Storage::read_block(void *dst, uint16_t loc, size_t n)
143 : {
144 74521223 : if (loc >= sizeof(_buffer)-(n-1)) {
145 0 : return;
146 : }
147 74521223 : _storage_open();
148 74521223 : memcpy(dst, &_buffer[loc], n);
149 : }
150 :
151 442006 : void Storage::write_block(uint16_t loc, const void *src, size_t n)
152 : {
153 442006 : if (loc >= sizeof(_buffer)-(n-1)) {
154 0 : return;
155 : }
156 442006 : if (memcmp(src, &_buffer[loc], n) != 0) {
157 92920 : _storage_open();
158 92920 : memcpy(&_buffer[loc], src, n);
159 92920 : _mark_dirty(loc, n);
160 : }
161 : }
162 :
163 243357861 : void Storage::_timer_tick(void)
164 : {
165 243357861 : if (_initialisedType == StorageBackend::None) {
166 0 : return;
167 : }
168 243357861 : if (_dirty_mask.empty()) {
169 243262104 : _last_empty_ms = AP_HAL::millis();
170 243262104 : return;
171 : }
172 :
173 : // write out the first dirty line. We don't write more
174 : // than one to keep the latency of this call to a minimum
175 : uint16_t i;
176 11071582 : for (i=0; i<STORAGE_NUM_LINES; i++) {
177 11071582 : if (_dirty_mask.get(i)) {
178 95757 : break;
179 : }
180 : }
181 95757 : if (i == STORAGE_NUM_LINES) {
182 : // this shouldn't be possible
183 0 : return;
184 : }
185 :
186 : #if STORAGE_USE_FRAM
187 95757 : if (fram.write(STORAGE_LINE_SIZE*i, &_buffer[STORAGE_LINE_SIZE*i], STORAGE_LINE_SIZE)) {
188 7 : _dirty_mask.clear(i);
189 7 : return;
190 : }
191 : #endif
192 :
193 : #if STORAGE_USE_POSIX
194 95750 : if (hal.get_storage_posix_enabled()) {
195 95687 : if (log_fd != -1) {
196 95687 : const off_t offset = STORAGE_LINE_SIZE*i;
197 95687 : if (lseek(log_fd, offset, SEEK_SET) != offset) {
198 0 : return;
199 : }
200 95687 : if (write(log_fd, &_buffer[offset], STORAGE_LINE_SIZE) != STORAGE_LINE_SIZE) {
201 0 : return;
202 : }
203 95687 : _dirty_mask.clear(i);
204 95687 : return;
205 : }
206 : }
207 : #endif
208 :
209 : #if STORAGE_USE_FLASH
210 63 : if (hal.get_storage_flash_enabled()) {
211 : // save to storage backend
212 63 : _flash_write(i);
213 63 : return;
214 : }
215 : #endif
216 : }
217 :
218 : #if STORAGE_USE_FLASH
219 :
220 : /*
221 : load all data from flash
222 : */
223 3 : void Storage::_flash_load(void)
224 : {
225 3 : if (!_flash.init()) {
226 0 : AP_HAL::panic("unable to init flash storage");
227 : }
228 3 : }
229 :
230 : /*
231 : write one storage line. This also updates _dirty_mask.
232 : */
233 63 : void Storage::_flash_write(uint16_t line)
234 : {
235 63 : if (_flash.write(line*STORAGE_LINE_SIZE, STORAGE_LINE_SIZE)) {
236 : // mark the line clean
237 63 : _dirty_mask.clear(line);
238 : }
239 63 : }
240 :
241 :
242 : /*
243 : emulate writing to flash
244 : */
245 : static int flash_fd = -1;
246 :
247 357 : static uint32_t sitl_flash_getpageaddr(uint32_t page)
248 : {
249 357 : return page * HAL_FLASH_SECTOR_SIZE;
250 : }
251 :
252 359 : static void sitl_flash_open(void)
253 : {
254 359 : if (flash_fd == -1) {
255 3 : flash_fd = open("flash.dat", O_RDWR, 0644);
256 3 : if (flash_fd == -1) {
257 1 : flash_fd = open("flash.dat", O_RDWR|O_CREAT, 0644);
258 1 : if (flash_fd == -1) {
259 0 : AP_HAL::panic("Failed to open flash.dat");
260 : }
261 1 : if (ftruncate(flash_fd, 2*HAL_FLASH_SECTOR_SIZE) != 0) {
262 0 : AP_HAL::panic("Failed to create flash.dat");
263 : }
264 : uint8_t fill[HAL_FLASH_SECTOR_SIZE*2];
265 1 : memset(fill, 0xff, sizeof(fill));
266 1 : if (pwrite(flash_fd, fill, sizeof(fill), 0) != (ssize_t)sizeof(fill)) {
267 0 : AP_HAL::panic("Failed to fill flash.dat");
268 : }
269 : }
270 : }
271 359 : }
272 :
273 80 : static bool sitl_flash_write(uint32_t addr, const uint8_t *data, uint32_t length)
274 : {
275 80 : sitl_flash_open();
276 80 : uint8_t old[length];
277 80 : if (pread(flash_fd, old, length, addr) != (ssize_t)length) {
278 0 : AP_HAL::panic("Failed to read flash.dat at %u len=%u", unsigned(addr), unsigned(length));
279 : }
280 : #if defined(HAL_FLASH_MIN_WRITE_SIZE) && HAL_FLASH_MIN_WRITE_SIZE == 32
281 80 : if ((length % HAL_FLASH_MIN_WRITE_SIZE) != 0 || (addr % HAL_FLASH_MIN_WRITE_SIZE) != 0) {
282 0 : AP_HAL::panic("Attempt to write flash at %u length %u", addr, length);
283 : }
284 : // emulate H7 requirement that writes be to untouched bytes
285 164 : for (uint32_t i=0; i<length; i+= 32) {
286 84 : if (memcmp(&old[i], &data[i], 32) == 0) {
287 3 : continue;
288 : }
289 2673 : for (uint32_t j=0; j<32; j++) {
290 2592 : if (old[i+j] != 0xFF) {
291 0 : AP_HAL::panic("Attempt to write modified flash at %u length %u", addr+i+j, length);
292 : }
293 : }
294 : }
295 : #endif
296 : // check that we are only ever clearing bits (real flash storage can only ever clear bits,
297 : // except for an erase
298 2768 : for (uint32_t i=0; i<length; i++) {
299 : #if HAL_FLASH_ALLOW_UPDATE
300 : // emulating flash that allows setting any bit from 1 to 0
301 : if (data[i] & ~old[i]) {
302 : AP_HAL::panic("Attempt to set flash byte 0x%02x from 0x%02x at %u", data[i], old[i], addr+i);
303 : }
304 : #else
305 : // emulating flash that only allows update if whole byte is 0xFF
306 2688 : if (data[i] != old[i] && old[i] != 0xFF) {
307 0 : AP_HAL::panic("Attempt to set flash byte 0x%02x from 0x%02x at %u", data[i], old[i], addr+i);
308 : }
309 : #endif
310 : }
311 80 : return pwrite(flash_fd, data, length, addr) == (ssize_t)length;
312 80 : }
313 :
314 277 : static bool sitl_flash_read(uint32_t addr, uint8_t *data, uint32_t length)
315 : {
316 277 : sitl_flash_open();
317 277 : return pread(flash_fd, data, length, addr) == (ssize_t)length;
318 : }
319 :
320 2 : static bool sitl_flash_erasepage(uint32_t page)
321 : {
322 : uint8_t fill[HAL_FLASH_SECTOR_SIZE];
323 2 : memset(fill, 0xff, sizeof(fill));
324 2 : sitl_flash_open();
325 2 : bool ret = pwrite(flash_fd, fill, sizeof(fill), page * HAL_FLASH_SECTOR_SIZE) == sizeof(fill);
326 2 : printf("erase %u -> %u\n", page, ret);
327 2 : return ret;
328 : }
329 :
330 : /*
331 : callback to write data to flash
332 : */
333 80 : bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
334 : {
335 80 : size_t base_address = sitl_flash_getpageaddr(sector);
336 80 : bool ret = sitl_flash_write(base_address+offset, data, length);
337 80 : if (!ret && _flash_erase_ok()) {
338 : // we are getting flash write errors while disarmed. Try
339 : // re-writing all of flash
340 0 : uint32_t now = AP_HAL::millis();
341 0 : if (now - _last_re_init_ms > 5000) {
342 0 : _last_re_init_ms = now;
343 0 : bool ok = _flash.re_initialise();
344 0 : hal.console->printf("Storage: failed at %u:%u for %u - re-init %u\n",
345 : (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok);
346 : }
347 : }
348 80 : return ret;
349 : }
350 :
351 : /*
352 : callback to read data from flash
353 : */
354 277 : bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
355 : {
356 277 : size_t base_address = sitl_flash_getpageaddr(sector);
357 277 : return sitl_flash_read(base_address+offset, data, length);
358 : }
359 :
360 : /*
361 : callback to erase flash sector
362 : */
363 2 : bool Storage::_flash_erase_sector(uint8_t sector)
364 : {
365 2 : return sitl_flash_erasepage(sector);
366 : }
367 :
368 : /*
369 : callback to check if erase is allowed
370 : */
371 0 : bool Storage::_flash_erase_ok(void)
372 : {
373 : // only allow erase while disarmed
374 0 : return !hal.util->get_soft_armed();
375 : }
376 :
377 : #endif // STORAGE_USE_FLASH
378 :
379 : /*
380 : consider storage healthy if we have nothing to write sometime in the
381 : last 2 seconds
382 : */
383 133080 : bool Storage::healthy(void)
384 : {
385 133080 : if (_initialisedType == StorageBackend::None) {
386 0 : return false;
387 : }
388 133080 : return AP_HAL::millis() - _last_empty_ms < 2000;
389 : }
390 :
391 : /*
392 : get storage size and ptr
393 : */
394 1259 : bool Storage::get_storage_ptr(void *&ptr, size_t &size)
395 : {
396 1259 : if (_initialisedType==StorageBackend::None) {
397 0 : return false;
398 : }
399 1259 : ptr = _buffer;
400 1259 : size = sizeof(_buffer);
401 1259 : return true;
402 : }
|
