2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2015-05-04 03:15:12 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2012-12-12 18:04:27 -04:00
|
|
|
|
|
|
|
#include <assert.h>
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/stat.h>
|
|
|
|
#include <fcntl.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
|
|
|
|
#include "Storage.h"
|
2015-05-04 03:15:12 -03:00
|
|
|
using namespace HALSITL;
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
void EEPROMStorage::_eeprom_open(void)
|
2012-12-12 18:04:27 -04:00
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
if (_eeprom_fd == -1) {
|
|
|
|
_eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT, 0777);
|
|
|
|
assert(ftruncate(_eeprom_fd, HAL_STORAGE_SIZE) == 0);
|
|
|
|
}
|
2012-12-12 18:04:27 -04:00
|
|
|
}
|
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
void EEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
|
2012-12-12 18:04:27 -04:00
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE);
|
|
|
|
_eeprom_open();
|
|
|
|
assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
|
2012-12-12 18:04:27 -04:00
|
|
|
}
|
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
void EEPROMStorage::write_block(uint16_t dst, const void *src, size_t n)
|
2012-12-12 18:04:27 -04:00
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
assert(dst < HAL_STORAGE_SIZE);
|
|
|
|
_eeprom_open();
|
|
|
|
assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n);
|
2012-12-12 18:04:27 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|