From e9165fdeab619b4ee1db1da1ec7f112042117f96 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 13 Jul 2021 11:37:51 +1000 Subject: [PATCH] SITL: add simulated RAMTRON device --- libraries/SITL/SIM_RAMTRON.cpp | 111 +++++++++++++++++++++++++ libraries/SITL/SIM_RAMTRON.h | 35 ++++++++ libraries/SITL/SIM_RAMTRON_FM25V02.cpp | 10 +++ libraries/SITL/SIM_RAMTRON_FM25V02.h | 33 ++++++++ libraries/SITL/SIM_SPI.cpp | 4 + 5 files changed, 193 insertions(+) create mode 100644 libraries/SITL/SIM_RAMTRON.cpp create mode 100644 libraries/SITL/SIM_RAMTRON.h create mode 100644 libraries/SITL/SIM_RAMTRON_FM25V02.cpp create mode 100644 libraries/SITL/SIM_RAMTRON_FM25V02.h diff --git a/libraries/SITL/SIM_RAMTRON.cpp b/libraries/SITL/SIM_RAMTRON.cpp new file mode 100644 index 0000000000..9a0fe368ac --- /dev/null +++ b/libraries/SITL/SIM_RAMTRON.cpp @@ -0,0 +1,111 @@ +#include "SIM_RAMTRON.h" + +#include +#include + +#include + +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 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; +} diff --git a/libraries/SITL/SIM_RAMTRON.h b/libraries/SITL/SIM_RAMTRON.h new file mode 100644 index 0000000000..c9215b9d68 --- /dev/null +++ b/libraries/SITL/SIM_RAMTRON.h @@ -0,0 +1,35 @@ +#pragma once + +#include "SIM_SPIDevice.h" + +namespace SITL { + +class RAMTRON : 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 uint32_t storage_size() const = 0; // in bytes + virtual const char *filename() const = 0; + +private: + + enum class State { + WAITING, + READING_RDID, + READING, + WRITING, + } state = State::WAITING; + + bool write_enabled; + uint32_t xfr_addr; + + void open_storage_fd(); + int storage_fd = -1; +}; + +} diff --git a/libraries/SITL/SIM_RAMTRON_FM25V02.cpp b/libraries/SITL/SIM_RAMTRON_FM25V02.cpp new file mode 100644 index 0000000000..c4b8c29af9 --- /dev/null +++ b/libraries/SITL/SIM_RAMTRON_FM25V02.cpp @@ -0,0 +1,10 @@ +#include "SIM_RAMTRON_FM25V02.h" + +using namespace SITL; + +void RAMTRON_FM25V02::fill_rdid(uint8_t *buffer, uint8_t len) +{ + memcpy(buffer, &manufacturer, ARRAY_SIZE(manufacturer)); + buffer[7] = id1(); + buffer[8] = id2(); +} diff --git a/libraries/SITL/SIM_RAMTRON_FM25V02.h b/libraries/SITL/SIM_RAMTRON_FM25V02.h new file mode 100644 index 0000000000..73502f5902 --- /dev/null +++ b/libraries/SITL/SIM_RAMTRON_FM25V02.h @@ -0,0 +1,33 @@ +#pragma once + +#include "SIM_RAMTRON.h" + +namespace SITL { + +class RAMTRON_FM25V02 : public RAMTRON +{ +protected: + + void fill_rdid(uint8_t *buf, uint8_t len) override; + + uint8_t id1() { return family << 5 | density; } + uint8_t id2() { return sub << 5 | rev << 2; } + + const char *filename() const override { return "RAMTRON-FM25V02.dat"; } + + uint32_t storage_size() const override { + return 32768; + } + +private: + + const uint8_t manufacturer[7] { 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0xC2 }; + static const uint8_t family = 1; + static const uint8_t density = 2; + static const uint8_t sub = 0; + static const uint8_t rev = 0; + static const uint8_t rsvd = 0; + +}; + +} diff --git a/libraries/SITL/SIM_SPI.cpp b/libraries/SITL/SIM_SPI.cpp index c49201f678..36beff4c48 100644 --- a/libraries/SITL/SIM_SPI.cpp +++ b/libraries/SITL/SIM_SPI.cpp @@ -21,16 +21,20 @@ #include #include "SIM_SPI.h" +#include "SIM_RAMTRON_FM25V02.h" #include using namespace SITL; +static RAMTRON_FM25V02 ramtron_FM25V02; // 32kB 2-byte-addressing + struct spi_device_at_cs_pin { uint8_t bus; uint8_t cs_pin; SPIDevice &device; } spi_devices[] { + { 0, 0, ramtron_FM25V02 }, }; void SPI::init()