mirror of https://github.com/ArduPilot/ardupilot
193 lines
5.7 KiB
C++
193 lines
5.7 KiB
C++
#include "SIM_JEDEC.h"
|
|
|
|
#include <errno.h>
|
|
#include <unistd.h>
|
|
|
|
#include <AP_HAL_SITL/AP_HAL_SITL.h>
|
|
|
|
using namespace SITL;
|
|
|
|
extern HAL_SITL& hal;
|
|
|
|
void JEDEC::open_storage_fd()
|
|
{
|
|
if (storage_fd != -1) {
|
|
AP_HAL::panic("Should not have been called");
|
|
}
|
|
const char *filepath = filename();
|
|
if (hal.get_wipe_storage()) {
|
|
unlink(filepath);
|
|
}
|
|
storage_fd = open(filepath, O_RDWR|O_CREAT, 0644);
|
|
if (storage_fd == -1) {
|
|
AP_HAL::panic("open(%s): %s", filepath, strerror(errno));
|
|
}
|
|
if (ftruncate(storage_fd, get_storage_size()) != 0) {
|
|
AP_HAL::panic("truncate(%s): %s", filepath, strerror(errno));
|
|
}
|
|
}
|
|
|
|
void JEDEC::sector4k_erase (uint32_t addr)
|
|
{
|
|
for (uint8_t i=0; i<get_page_per_sector(); i++) {
|
|
page_erase(addr + i*get_page_size());
|
|
}
|
|
}
|
|
|
|
void JEDEC::block64k_erase (uint32_t addr)
|
|
{
|
|
// we have 16 sectors in a block
|
|
for (uint16_t i=0; i<16; i++) {
|
|
sector4k_erase(addr + i*get_page_per_sector()*get_page_size());
|
|
}
|
|
}
|
|
|
|
void JEDEC::page_erase (uint32_t addr)
|
|
{
|
|
const uint32_t fill_length = get_page_size();
|
|
uint8_t fill[fill_length];
|
|
if (addr + fill_length > get_storage_size()) {
|
|
AP_HAL::panic("trying to write outside memory");
|
|
}
|
|
memset(fill, 0xFF, sizeof(fill));
|
|
const size_t write_ret = pwrite(storage_fd, fill, sizeof(fill), addr);
|
|
if (write_ret != sizeof(fill)) {
|
|
printf("Failed page erase");
|
|
}
|
|
}
|
|
|
|
void JEDEC::bulk_erase()
|
|
{
|
|
for (uint16_t i=0; i<get_num_blocks(); i++) {
|
|
block64k_erase(i*get_page_per_block()*get_page_size());
|
|
}
|
|
}
|
|
|
|
uint32_t JEDEC::parse_addr (uint8_t* buffer, uint32_t len)
|
|
{
|
|
if (len<4) {
|
|
AP_HAL::panic("address too short");
|
|
}
|
|
|
|
// buffer[0] is cmd
|
|
return buffer[1] << 16 | buffer[2] << 8 | buffer[3];
|
|
}
|
|
|
|
void JEDEC::assert_writes_enabled()
|
|
{
|
|
if (!write_enabled) {
|
|
AP_HAL::panic("Writes not enabled");
|
|
}
|
|
}
|
|
|
|
int JEDEC::rdwr(uint8_t count, SPI::spi_ioc_transfer *&tfrs)
|
|
{
|
|
if (storage_fd == -1) {
|
|
open_storage_fd();
|
|
}
|
|
|
|
// commands:
|
|
static const uint8_t JEDEC_RDID = 0x9f;
|
|
static const uint8_t JEDEC_READ = 0x03;
|
|
static const uint8_t JEDEC_WREN = 0x06;
|
|
static const uint8_t JEDEC_WRITE = 0x02;
|
|
static const uint8_t JEDEC_RDSR = 0x05;
|
|
static const uint8_t JEDEC_SECTOR4_ERASE = 0x20;
|
|
static const uint8_t JEDEC_BULK_ERASE = 0xC7;
|
|
static const uint8_t JEDEC_BLOCK64_ERASE = 0xD8;
|
|
|
|
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 JEDEC_RDID:
|
|
state = State::READING_RDID;
|
|
break;
|
|
case JEDEC_READ:
|
|
xfr_addr = parse_addr(tx_buf, tfr.len);
|
|
state = State::READING;
|
|
break;
|
|
case JEDEC_WRITE:
|
|
xfr_addr = parse_addr(tx_buf, tfr.len);
|
|
state = State::WRITING;
|
|
break;
|
|
case JEDEC_WREN:
|
|
write_enabled = true;
|
|
break;
|
|
case JEDEC_RDSR:
|
|
state = State::READING_RDSR;
|
|
break;
|
|
case JEDEC_SECTOR4_ERASE: {
|
|
xfr_addr = parse_addr(tx_buf, tfr.len);
|
|
assert_writes_enabled();
|
|
sector4k_erase(xfr_addr);
|
|
write_enabled = false;
|
|
break;
|
|
}
|
|
case JEDEC_BULK_ERASE: {
|
|
assert_writes_enabled();
|
|
bulk_erase();
|
|
write_enabled = false;
|
|
break;
|
|
}
|
|
case JEDEC_BLOCK64_ERASE: {
|
|
xfr_addr = parse_addr(tx_buf, tfr.len);
|
|
assert_writes_enabled();
|
|
block64k_erase(xfr_addr);
|
|
write_enabled = false;
|
|
break;
|
|
}
|
|
default:
|
|
AP_HAL::panic("Unhandled command received");
|
|
}
|
|
break;
|
|
}
|
|
case State::READING_RDID:
|
|
fill_rdid(rx_buf, tfr.len);
|
|
state = State::WAITING;
|
|
break;
|
|
case State::READING_RDSR:
|
|
fill_rdsr(rx_buf, tfr.len);
|
|
state = State::WAITING;
|
|
break;
|
|
case State::READING: {
|
|
if (xfr_addr + tfr.len > get_storage_size()) {
|
|
AP_HAL::panic("trying to read outside memory");
|
|
}
|
|
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: {
|
|
assert_writes_enabled();
|
|
if (xfr_addr + tfr.len > get_storage_size()) {
|
|
AP_HAL::panic("trying to write outside memory");
|
|
}
|
|
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;
|
|
}
|