mirror of https://github.com/ArduPilot/ardupilot
112 lines
3.3 KiB
C++
112 lines
3.3 KiB
C++
#include "SIM_RAMTRON.h"
|
|
|
|
#include <errno.h>
|
|
#include <unistd.h>
|
|
|
|
#include <AP_HAL_SITL/AP_HAL_SITL.h>
|
|
|
|
using namespace SITL;
|
|
|
|
extern HAL_SITL& hal;
|
|
|
|
void RAMTRON::open_storage_fd()
|
|
{
|
|
if (storage_fd != -1) {
|
|
AP_HAL::panic("Should not have been called");
|
|
}
|
|
const char *filepath = filename();
|
|
uint32_t flags = O_RDWR|O_CREAT;
|
|
if (hal.get_wipe_storage()) {
|
|
flags |= O_TRUNC;
|
|
}
|
|
storage_fd = open(filepath, flags, 0644);
|
|
if (storage_fd == -1) {
|
|
AP_HAL::panic("open(%s): %s", filepath, strerror(errno));
|
|
}
|
|
if (ftruncate(storage_fd, storage_size()) != 0) {
|
|
AP_HAL::panic("truncate(%s): %s", filepath, strerror(errno));
|
|
}
|
|
}
|
|
|
|
int RAMTRON::rdwr(uint8_t count, SPI::spi_ioc_transfer *&tfrs)
|
|
{
|
|
if (storage_fd == -1) {
|
|
open_storage_fd();
|
|
}
|
|
|
|
// commands:
|
|
static const uint8_t RAMTRON_RDID = 0x9f;
|
|
static const uint8_t RAMTRON_READ = 0x03;
|
|
static const uint8_t RAMTRON_WREN = 0x06;
|
|
static const uint8_t RAMTRON_WRITE = 0x02;
|
|
|
|
for (uint8_t i=0; i<count; i++) {
|
|
SPI::spi_ioc_transfer &tfr = tfrs[i];
|
|
uint8_t *tx_buf = (uint8_t*)(tfr.tx_buf);
|
|
uint8_t *rx_buf = (uint8_t*)(tfr.rx_buf);
|
|
|
|
switch (state) {
|
|
case State::WAITING: {
|
|
// find a command
|
|
uint8_t command = tx_buf[0];
|
|
switch (command) {
|
|
case RAMTRON_RDID:
|
|
state = State::READING_RDID;
|
|
break;
|
|
case RAMTRON_READ:
|
|
xfr_addr = tx_buf[1] << 8 | tx_buf[2];
|
|
state = State::READING;
|
|
break;
|
|
case RAMTRON_WRITE:
|
|
xfr_addr = tx_buf[1] << 8 | tx_buf[2];
|
|
state = State::WRITING;
|
|
break;
|
|
case RAMTRON_WREN:
|
|
write_enabled = true;
|
|
break;
|
|
default:
|
|
abort();
|
|
}
|
|
break;
|
|
}
|
|
case State::READING_RDID:
|
|
fill_rdid(rx_buf, tfr.len);
|
|
state = State::WAITING;
|
|
break;
|
|
case State::READING: {
|
|
if (xfr_addr + tfr.len > storage_size()) {
|
|
abort();
|
|
}
|
|
if (lseek(storage_fd, xfr_addr, SEEK_SET) == -1) {
|
|
AP_HAL::panic("lseek(): %s", strerror(errno));
|
|
}
|
|
const size_t read_ret = read(storage_fd, rx_buf, tfr.len);
|
|
if (read_ret != tfr.len) {
|
|
AP_HAL::panic("read(): %s (%d/%u)", strerror(errno), (signed)read_ret, (unsigned)tfr.len);
|
|
}
|
|
state = State::WAITING;
|
|
break;
|
|
}
|
|
case State::WRITING: {
|
|
if (!write_enabled) {
|
|
AP_HAL::panic("Writes not enabled");
|
|
}
|
|
if (xfr_addr + tfr.len > storage_size()) {
|
|
abort();
|
|
}
|
|
if (lseek(storage_fd, xfr_addr, SEEK_SET) == -1) {
|
|
AP_HAL::panic("lseek(): %s", strerror(errno));
|
|
}
|
|
const size_t write_ret = write(storage_fd, tx_buf, tfr.len);
|
|
if (write_ret != tfr.len) {
|
|
AP_HAL::panic("write(): %s (%d/%u)", strerror(errno), (signed)write_ret, (unsigned)tfr.len);
|
|
}
|
|
state = State::WAITING;
|
|
write_enabled = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|