ardupilot/libraries/AP_HAL_QURT/Storage.cpp

#include "Storage.h"

#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>

#include <AP_HAL/AP_HAL.h>

using namespace QURT;

#define QURT_STORAGE_MAX_WRITE 512
#define QURT_STORAGE_LINE_SHIFT 9
#define QURT_STORAGE_LINE_SIZE (1<<QURT_STORAGE_LINE_SHIFT)
#define QURT_STORAGE_NUM_LINES (QURT_STORAGE_SIZE/QURT_STORAGE_LINE_SIZE)

/*
  This stores 'eeprom' data on the SD card, with a 4k size, and a
  in-memory buffer. This keeps the latency down.
 */

// name the storage file after the sketch so you can use the same board
// card for ArduCopter and ArduPlane
#define STORAGE_FILE AP_FILESYSTEM_POSIX_MAP_FILENAME_BASEDIR "/APM/" AP_BUILD_TARGET_NAME ".stg"

extern const AP_HAL::HAL& hal;

bool Storage::_storage_create(void)
{
    int fd = -1;

    fd = open(STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0755);
    if (fd == -1) {
        AP_HAL::panic("Failed to open storage: %s", STORAGE_FILE);
        return false;
    }

    // take up all needed space
    if (write(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
        AP_HAL::panic("Failed to init %s", STORAGE_FILE);
        goto fail;
    }

    // ensure the directory is updated with the new size
    close(fd);

    return true;

fail:
    if (fd != -1) {
        close(fd);
    }
    return false;
}

void Storage::init()
{
    if (_initialised) {
        return;
    }

    _dirty_mask = 0;

    int fd = open(STORAGE_FILE, O_RDWR);
    if (fd == -1) {
        _storage_create();
        fd = open(STORAGE_FILE, O_RDWR);
        if (fd == -1) {
            AP_HAL::panic("Failed to create %s", STORAGE_FILE);
        }
    }

    ssize_t ret = read(fd, _buffer, sizeof(_buffer));

    if (ret != sizeof(_buffer)) {
        close(fd);
        _storage_create();
        fd = open(STORAGE_FILE, O_RDWR);
        if (fd == -1) {
            AP_HAL::panic("Failed to open %s", STORAGE_FILE);
        }
        if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
            AP_HAL::panic("Failed to read %s", STORAGE_FILE);
        }
    }

    _fd = fd;
    _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 Storage::_mark_dirty(uint16_t loc, uint16_t length)
{
    if (length == 0) {
        return;
    }
    uint16_t end = loc + length - 1;
    for (uint8_t line=loc>>QURT_STORAGE_LINE_SHIFT;
         line <= end>>QURT_STORAGE_LINE_SHIFT;
         line++) {
        _dirty_mask |= 1U << line;
    }
}

void Storage::read_block(void *dst, uint16_t loc, size_t n)
{
    if (loc >= sizeof(_buffer)-(n-1)) {
        return;
    }
    init();
    memcpy(dst, &_buffer[loc], n);
}

void Storage::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) {
        init();
        memcpy(&_buffer[loc], src, n);
        _mark_dirty(loc, n);
    }
}

void Storage::_timer_tick(void)
{
    if (!_initialised || _dirty_mask == 0 || _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;
    for (i=0; i<QURT_STORAGE_NUM_LINES; i++) {
        if (_dirty_mask & (1<<i)) {
            break;
        }
    }
    if (i == QURT_STORAGE_NUM_LINES) {
        // this shouldn't be possible
        return;
    }
    uint32_t write_mask = (1U<<i);
    // see how many lines to write
    for (n=1; (i+n) < QURT_STORAGE_NUM_LINES &&
         n < (QURT_STORAGE_MAX_WRITE>>QURT_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<<QURT_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<QURT_STORAGE_LINE_SHIFT)) {
        _dirty_mask &= ~write_mask;
        if (write(_fd, &_buffer[i<<QURT_STORAGE_LINE_SHIFT], n<<QURT_STORAGE_LINE_SHIFT) != n<<QURT_STORAGE_LINE_SHIFT) {
            // write error - likely EINTR
            _dirty_mask |= write_mask;
            close(_fd);
            _fd = -1;
        }
        if (_dirty_mask == 0) {
            // close and re-open to force storage to sync, QURT has no fsync() call
            close(_fd);
            _fd = open(STORAGE_FILE, O_RDWR);
        }
    }
}

/*
  get storage size and ptr
 */
bool Storage::get_storage_ptr(void *&ptr, size_t &size)
{
    if (!_initialised) {
        return false;
    }
    ptr = _buffer;
    size = sizeof(_buffer);
    return true;
}