SITL: add JEDEC dataflash simulator
We currently use AP_Logger_SITL for this purpose but we can instead add a dataflash simulator which can work with existing AP_Logger_Dataflash backend instead of using AP_Logger_SITL. Co-Authored-By: Divyateja Pasupuleti <divyateja2004@gmail.com>
This commit is contained in:
Shiv Tyagi
Peter Barker
parent
3533e1e0ed
commit
fa6adc0beb
192
libraries/SITL/SIM_JEDEC.cpp
Normal file
192
libraries/SITL/SIM_JEDEC.cpp
Normal file
@ -0,0 +1,192 @@
|
||||
#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;
|
||||
}
|
||||
48
libraries/SITL/SIM_JEDEC.h
Normal file
48
libraries/SITL/SIM_JEDEC.h
Normal file
@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
|
||||
#include "SIM_SPIDevice.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class JEDEC : public SPIDevice
|
||||
{
|
||||
public:
|
||||
|
||||
int rdwr(uint8_t count, SPI::spi_ioc_transfer *&data) override;
|
||||
|
||||
protected:
|
||||
|
||||
virtual void fill_rdid(uint8_t *buf, uint8_t len) = 0;
|
||||
virtual void fill_rdsr(uint8_t *buf, uint8_t len) = 0;
|
||||
virtual const char *filename() const = 0;
|
||||
virtual uint8_t get_num_blocks() const = 0;
|
||||
virtual uint16_t get_page_per_block() const = 0;
|
||||
virtual uint8_t get_page_per_sector() const = 0;
|
||||
virtual uint16_t get_page_size() const = 0;
|
||||
uint32_t get_storage_size() const { return get_num_pages()*get_page_size(); } // in bytes
|
||||
uint32_t get_num_pages() const { return get_num_blocks()*get_page_per_block(); }
|
||||
|
||||
private:
|
||||
|
||||
enum class State {
|
||||
WAITING,
|
||||
READING_RDID,
|
||||
READING,
|
||||
WRITING,
|
||||
READING_RDSR
|
||||
} state = State::WAITING;
|
||||
|
||||
bool write_enabled;
|
||||
uint32_t xfr_addr;
|
||||
|
||||
void sector4k_erase(uint32_t addr);
|
||||
void block64k_erase(uint32_t addr);
|
||||
void page_erase(uint32_t addr);
|
||||
void bulk_erase();
|
||||
uint32_t parse_addr(uint8_t *buffer, uint32_t len);
|
||||
void assert_writes_enabled();
|
||||
void open_storage_fd();
|
||||
int storage_fd = -1;
|
||||
};
|
||||
|
||||
}
|
||||
17
libraries/SITL/SIM_JEDEC_MX25L3206E.cpp
Normal file
17
libraries/SITL/SIM_JEDEC_MX25L3206E.cpp
Normal file
@ -0,0 +1,17 @@
|
||||
#include "SIM_JEDEC_MX25L3206E.h"
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
void JEDEC_MX25L3206E::fill_rdid(uint8_t *buffer, uint8_t len)
|
||||
{
|
||||
buffer[0] = 0xC2;
|
||||
buffer[1] = type;
|
||||
buffer[2] = density;
|
||||
}
|
||||
|
||||
void JEDEC_MX25L3206E::fill_rdsr(uint8_t *buffer, uint8_t len)
|
||||
{
|
||||
// we never allow multiple operations at a time
|
||||
// hence we can never be busy while reading the status register
|
||||
buffer[0] = 0x00;
|
||||
}
|
||||
26
libraries/SITL/SIM_JEDEC_MX25L3206E.h
Normal file
26
libraries/SITL/SIM_JEDEC_MX25L3206E.h
Normal file
@ -0,0 +1,26 @@
|
||||
#pragma once
|
||||
|
||||
#include "SIM_JEDEC.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class JEDEC_MX25L3206E : public JEDEC
|
||||
{
|
||||
protected:
|
||||
|
||||
void fill_rdid(uint8_t *buf, uint8_t len) override;
|
||||
void fill_rdsr(uint8_t *buf, uint8_t len) override;
|
||||
|
||||
const char *filename() const override { return "blackbox.bin"; }
|
||||
uint8_t get_num_blocks() const override { return 64; }
|
||||
uint16_t get_page_per_block() const override { return 256; }
|
||||
uint8_t get_page_per_sector() const override { return 16; }
|
||||
uint16_t get_page_size() const override { return 256; }
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t type = 0x20;
|
||||
static const uint8_t density = 0x16;
|
||||
};
|
||||
|
||||
}
|
||||
@ -22,12 +22,14 @@
|
||||
|
||||
#include "SIM_SPI.h"
|
||||
#include "SIM_RAMTRON_FM25V02.h"
|
||||
#include "SIM_JEDEC_MX25L3206E.h"
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
static RAMTRON_FM25V02 ramtron_FM25V02; // 32kB 2-byte-addressing
|
||||
static JEDEC_MX25L3206E jedec_MX25L3206E;
|
||||
|
||||
struct spi_device_at_cs_pin {
|
||||
uint8_t bus;
|
||||
@ -35,6 +37,7 @@ struct spi_device_at_cs_pin {
|
||||
SPIDevice &device;
|
||||
} spi_devices[] {
|
||||
{ 0, 0, ramtron_FM25V02 },
|
||||
{ 1, 0, jedec_MX25L3206E },
|
||||
};
|
||||
|
||||
void SPI::init()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user