mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-07 16:38:30 -04:00
7fede90df3
Pixracer has FRAM on the same bus as the ms5611 and the FRAM ramtron driver does not use the same locking mechanism as other px4 SPI drivers. We need to disable interrupts during FRAM transfers to ensure we don't get FRAM corruption
310 lines
8.7 KiB
C++
310 lines
8.7 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
|
|
#include <assert.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <stdio.h>
|
|
#include <ctype.h>
|
|
|
|
#include "Storage.h"
|
|
using namespace PX4;
|
|
|
|
/*
|
|
This stores eeprom data in the PX4 MTD interface with a 4k size, and
|
|
a in-memory buffer. This keeps the latency and devices IOs down.
|
|
*/
|
|
|
|
// name the storage file after the sketch so you can use the same sd
|
|
// card for ArduCopter and ArduPlane
|
|
#define STORAGE_DIR "/fs/microsd/APM"
|
|
#define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
|
|
#define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak"
|
|
//#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav"
|
|
#define MTD_PARAMS_FILE "/fs/mtd"
|
|
#define MTD_SIGNATURE 0x14012014
|
|
#define MTD_SIGNATURE_OFFSET (8192-4)
|
|
#define STORAGE_RENAME_OLD_FILE 0
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
PX4Storage::PX4Storage(void) :
|
|
_fd(-1),
|
|
_dirty_mask(0),
|
|
_perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")),
|
|
_perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors"))
|
|
{
|
|
}
|
|
|
|
/*
|
|
get signature from bytes at offset MTD_SIGNATURE_OFFSET
|
|
*/
|
|
uint32_t PX4Storage::_mtd_signature(void)
|
|
{
|
|
int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY);
|
|
if (mtd_fd == -1) {
|
|
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
|
|
}
|
|
uint32_t v;
|
|
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
|
|
AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
|
|
}
|
|
bus_lock(true);
|
|
if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
|
|
AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE);
|
|
}
|
|
bus_lock(false);
|
|
close(mtd_fd);
|
|
return v;
|
|
}
|
|
|
|
/*
|
|
put signature bytes at offset MTD_SIGNATURE_OFFSET
|
|
*/
|
|
void PX4Storage::_mtd_write_signature(void)
|
|
{
|
|
int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
|
|
if (mtd_fd == -1) {
|
|
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
|
|
}
|
|
uint32_t v = MTD_SIGNATURE;
|
|
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
|
|
AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
|
|
}
|
|
bus_lock(true);
|
|
if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
|
|
AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE);
|
|
}
|
|
bus_lock(false);
|
|
close(mtd_fd);
|
|
}
|
|
|
|
/*
|
|
upgrade from microSD to MTD (FRAM)
|
|
*/
|
|
void PX4Storage::_upgrade_to_mtd(void)
|
|
{
|
|
// the MTD is completely uninitialised - try to get a
|
|
// copy from OLD_STORAGE_FILE
|
|
int old_fd = open(OLD_STORAGE_FILE, O_RDONLY);
|
|
if (old_fd == -1) {
|
|
::printf("Failed to open %s\n", OLD_STORAGE_FILE);
|
|
return;
|
|
}
|
|
|
|
int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
|
|
if (mtd_fd == -1) {
|
|
AP_HAL::panic("Unable to open MTD for upgrade");
|
|
}
|
|
|
|
if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
|
|
close(old_fd);
|
|
close(mtd_fd);
|
|
::printf("Failed to read %s\n", OLD_STORAGE_FILE);
|
|
return;
|
|
}
|
|
close(old_fd);
|
|
ssize_t ret;
|
|
bus_lock(true);
|
|
if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
|
|
::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
|
|
AP_HAL::panic("Unable to write MTD for upgrade");
|
|
}
|
|
bus_lock(false);
|
|
close(mtd_fd);
|
|
#if STORAGE_RENAME_OLD_FILE
|
|
rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
|
|
#endif
|
|
::printf("Upgraded MTD from %s\n", OLD_STORAGE_FILE);
|
|
}
|
|
|
|
|
|
void PX4Storage::_storage_open(void)
|
|
{
|
|
if (_initialised) {
|
|
return;
|
|
}
|
|
|
|
struct stat st;
|
|
_have_mtd = (stat(MTD_PARAMS_FILE, &st) == 0);
|
|
|
|
// PX4 should always have /fs/mtd_params
|
|
if (!_have_mtd) {
|
|
AP_HAL::panic("Failed to find " MTD_PARAMS_FILE);
|
|
}
|
|
|
|
/*
|
|
cope with upgrading from OLD_STORAGE_FILE to MTD
|
|
*/
|
|
bool good_signature = (_mtd_signature() == MTD_SIGNATURE);
|
|
if (stat(OLD_STORAGE_FILE, &st) == 0) {
|
|
if (good_signature) {
|
|
#if STORAGE_RENAME_OLD_FILE
|
|
rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
|
|
#endif
|
|
} else {
|
|
_upgrade_to_mtd();
|
|
}
|
|
}
|
|
|
|
// we write the signature every time, even if it already is
|
|
// good, as this gives us a way to detect if the MTD device is
|
|
// functional. It is better to panic now than to fail to save
|
|
// parameters in flight
|
|
_mtd_write_signature();
|
|
|
|
_dirty_mask = 0;
|
|
int fd = open(MTD_PARAMS_FILE, O_RDONLY);
|
|
if (fd == -1) {
|
|
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
|
|
}
|
|
const uint16_t chunk_size = 128;
|
|
for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
|
|
bus_lock(true);
|
|
ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
|
|
bus_lock(false);
|
|
if (ret != chunk_size) {
|
|
::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
|
|
(unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
|
|
AP_HAL::panic("Failed to read " MTD_PARAMS_FILE);
|
|
}
|
|
}
|
|
close(fd);
|
|
|
|
#ifdef SAVE_STORAGE_FILE
|
|
fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644);
|
|
if (fd != -1) {
|
|
write(fd, _buffer, sizeof(_buffer));
|
|
close(fd);
|
|
::printf("Saved storage file %s\n", SAVE_STORAGE_FILE);
|
|
}
|
|
#endif
|
|
_initialised = true;
|
|
}
|
|
|
|
/*
|
|
mark some lines as dirty. Note that there is no attempt to avoid
|
|
the race condition between this code and the _timer_tick() code
|
|
below, which both update _dirty_mask. If we lose the race then the
|
|
result is that a line is written more than once, but it won't result
|
|
in a line not being written.
|
|
*/
|
|
void PX4Storage::_mark_dirty(uint16_t loc, uint16_t length)
|
|
{
|
|
uint16_t end = loc + length;
|
|
for (uint8_t line=loc>>PX4_STORAGE_LINE_SHIFT;
|
|
line <= end>>PX4_STORAGE_LINE_SHIFT;
|
|
line++) {
|
|
_dirty_mask |= 1U << line;
|
|
}
|
|
}
|
|
|
|
void PX4Storage::read_block(void *dst, uint16_t loc, size_t n)
|
|
{
|
|
if (loc >= sizeof(_buffer)-(n-1)) {
|
|
return;
|
|
}
|
|
_storage_open();
|
|
memcpy(dst, &_buffer[loc], n);
|
|
}
|
|
|
|
void PX4Storage::write_block(uint16_t loc, const void *src, size_t n)
|
|
{
|
|
if (loc >= sizeof(_buffer)-(n-1)) {
|
|
return;
|
|
}
|
|
if (memcmp(src, &_buffer[loc], n) != 0) {
|
|
_storage_open();
|
|
memcpy(&_buffer[loc], src, n);
|
|
_mark_dirty(loc, n);
|
|
}
|
|
}
|
|
|
|
void PX4Storage::bus_lock(bool lock)
|
|
{
|
|
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
|
|
/*
|
|
this is needed on Pixracer where the ms5611 may be on the same
|
|
bus as FRAM, and the NuttX ramtron driver does not go via the
|
|
PX4 spi bus abstraction. The ramtron driver relies on
|
|
SPI_LOCK(). We need to prevent the ms5611 reads which happen in
|
|
interrupt context from interfering with the FRAM operations. As
|
|
the px4 spi bus abstraction just uses interrupt blocking as the
|
|
locking mechanism we need to block interrupts here as well.
|
|
*/
|
|
if (lock) {
|
|
irq_state = irqsave();
|
|
} else {
|
|
irqrestore(irq_state);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void PX4Storage::_timer_tick(void)
|
|
{
|
|
if (!_initialised || _dirty_mask == 0) {
|
|
return;
|
|
}
|
|
perf_begin(_perf_storage);
|
|
|
|
if (_fd == -1) {
|
|
_fd = open(MTD_PARAMS_FILE, O_WRONLY);
|
|
if (_fd == -1) {
|
|
perf_end(_perf_storage);
|
|
perf_count(_perf_errors);
|
|
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;
|
|
for (i=0; i<PX4_STORAGE_NUM_LINES; i++) {
|
|
if (_dirty_mask & (1<<i)) {
|
|
break;
|
|
}
|
|
}
|
|
if (i == PX4_STORAGE_NUM_LINES) {
|
|
// this shouldn't be possible
|
|
perf_end(_perf_storage);
|
|
perf_count(_perf_errors);
|
|
return;
|
|
}
|
|
uint32_t write_mask = (1U<<i);
|
|
// see how many lines to write
|
|
for (n=1; (i+n) < PX4_STORAGE_NUM_LINES &&
|
|
n < (PX4_STORAGE_MAX_WRITE>>PX4_STORAGE_LINE_SHIFT); n++) {
|
|
if (!(_dirty_mask & (1<<(n+i)))) {
|
|
break;
|
|
}
|
|
// mark that line clean
|
|
write_mask |= (1<<(n+i));
|
|
}
|
|
|
|
/*
|
|
write the lines. This also updates _dirty_mask. Note that
|
|
because this is a SCHED_FIFO thread it will not be preempted
|
|
by the main task except during blocking calls. This means we
|
|
don't need a semaphore around the _dirty_mask updates.
|
|
*/
|
|
if (lseek(_fd, i<<PX4_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<PX4_STORAGE_LINE_SHIFT)) {
|
|
_dirty_mask &= ~write_mask;
|
|
bus_lock(true);
|
|
ssize_t ret = write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT);
|
|
bus_lock(false);
|
|
if (ret != n<<PX4_STORAGE_LINE_SHIFT) {
|
|
// write error - likely EINTR
|
|
_dirty_mask |= write_mask;
|
|
close(_fd);
|
|
_fd = -1;
|
|
perf_count(_perf_errors);
|
|
}
|
|
}
|
|
perf_end(_perf_storage);
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|