diff --git a/libraries/AP_HAL_Linux/Storage.cpp b/libraries/AP_HAL_Linux/Storage.cpp index 40d9f288c7..26d56b8df6 100644 --- a/libraries/AP_HAL_Linux/Storage.cpp +++ b/libraries/AP_HAL_Linux/Storage.cpp @@ -91,78 +91,86 @@ static int mkdir_p(const char *path, int len, mode_t mode) return 0; } -void Storage::_storage_create(void) +int Storage::_storage_create(const char *dpath) { - const char *dpath = HAL_BOARD_STORAGE_DIRECTORY, *p; int dfd = -1; - p = hal.util->get_custom_storage_directory(); - if (p) { - dpath = p; - } - mkdir_p(dpath, strlen(dpath), 0777); - dfd = open(dpath, O_RDWR|O_CLOEXEC); - if (dfd < 0) { - AP_HAL::panic("Error opening storage directory: %s\n", dpath); + dfd = open(dpath, O_RDONLY|O_CLOEXEC); + if (dfd == -1) { + fprintf(stderr, "Failed to open storage directory: %s (%m)\n", dpath); + return -1; } - unlinkat(dfd, STORAGE_FILE, 0); + unlinkat(dfd, dpath, 0); int fd = openat(dfd, STORAGE_FILE, O_RDWR|O_CREAT|O_CLOEXEC, 0666); close(dfd); if (fd == -1) { - AP_HAL::panic("Failed to create %s/%s", HAL_BOARD_STORAGE_DIRECTORY, - STORAGE_FILE); + fprintf(stderr, "Failed to create storage file %s/%s\n", dpath, + STORAGE_FILE); + goto fail; + } + + // take up all needed space + if (ftruncate(fd, sizeof(_buffer)) == -1) { + fprintf(stderr, "Failed to set file size to %u kB (%m)\n", + sizeof(_buffer) / 1024); + goto fail; } // ensure the directory is updated with the new size fsync(fd); - close(fd); + fsync(dfd); + + close(dfd); + + return fd; + +fail: + close(dfd); + return -1; } -void Storage::_storage_open(void) +void Storage::init() { + const char *dpath; + if (_initialised) { return; } _dirty_mask = 0; - memset(_buffer, 0, sizeof(_buffer)); - int fd = open(STORAGE_FILE, O_RDWR|O_CLOEXEC); + dpath = hal.util->get_custom_storage_directory(); + if (!dpath) { + dpath = HAL_BOARD_STORAGE_DIRECTORY; + } + + int fd = open(dpath, O_RDWR|O_CLOEXEC); if (fd == -1) { - _storage_create(); - fd = open(STORAGE_FILE, O_RDWR|O_CLOEXEC); + fd = _storage_create(dpath); if (fd == -1) { - AP_HAL::panic("Failed to open " STORAGE_FILE); + AP_HAL::panic("Cannot create storage %s (%m)", dpath); } } - /* - we allow a read of size 4096 to cope with the old storage size - without forcing users to reset all parameters - */ ssize_t ret = read(fd, _buffer, sizeof(_buffer)); - if (ret == 4096 && ret != sizeof(_buffer)) { - if (ftruncate(fd, sizeof(_buffer)) != 0) { - AP_HAL::panic("Failed to expand " STORAGE_FILE); - } - ret = sizeof(_buffer); - } + if (ret != sizeof(_buffer)) { close(fd); - _storage_create(); - fd = open(STORAGE_FILE, O_RDONLY|O_CLOEXEC); + _storage_create(dpath); + fd = open(dpath, O_RDONLY|O_CLOEXEC); if (fd == -1) { - AP_HAL::panic("Failed to open " STORAGE_FILE); + AP_HAL::panic("Failed to open %s (%m)", dpath); } if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { - AP_HAL::panic("Failed to read " STORAGE_FILE); + AP_HAL::panic("Failed to read %s (%m)", dpath); } } - close(fd); + + _fd = fd; _initialised = true; } @@ -188,7 +196,7 @@ void Storage::read_block(void *dst, uint16_t loc, size_t n) if (loc >= sizeof(_buffer)-(n-1)) { return; } - _storage_open(); + init(); memcpy(dst, &_buffer[loc], n); } @@ -198,7 +206,7 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n) return; } if (memcmp(src, &_buffer[loc], n) != 0) { - _storage_open(); + init(); memcpy(&_buffer[loc], src, n); _mark_dirty(loc, n); } @@ -206,17 +214,10 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n) void Storage::_timer_tick(void) { - if (!_initialised || _dirty_mask == 0) { + if (!_initialised || _dirty_mask == 0 || _fd == -1) { return; } - if (_fd == -1) { - _fd = open(STORAGE_FILE, O_WRONLY|O_CLOEXEC); - if (_fd == -1) { - return; - } - } - // write out the first dirty set of lines. We don't write more // than one to keep the latency of this call to a minimum uint8_t i, n; diff --git a/libraries/AP_HAL_Linux/Storage.h b/libraries/AP_HAL_Linux/Storage.h index 0262bb8033..fd4a2447b4 100644 --- a/libraries/AP_HAL_Linux/Storage.h +++ b/libraries/AP_HAL_Linux/Storage.h @@ -19,7 +19,9 @@ public: return static_cast(storage); } - void init() {} + + void init() override; + uint8_t read_byte(uint16_t loc); uint16_t read_word(uint16_t loc); uint32_t read_dword(uint16_t loc); @@ -31,15 +33,15 @@ public: void write_block(uint16_t dst, const void* src, size_t n); virtual void _timer_tick(void) override; - + protected: void _mark_dirty(uint16_t loc, uint16_t length); - virtual void _storage_create(void); - virtual void _storage_open(void); + int _storage_create(const char *dpath); + int _fd; volatile bool _initialised; - uint8_t _buffer[LINUX_STORAGE_SIZE]; volatile uint32_t _dirty_mask; + uint8_t _buffer[LINUX_STORAGE_SIZE]; }; }