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>

libraries/SITL/SIM_JEDEC.cpp

@@ -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;
+}

libraries/SITL/SIM_JEDEC.h

@@ -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;
+};
+
+}

libraries/SITL/SIM_JEDEC_MX25L3206E.cpp

@@ -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;
+}

libraries/SITL/SIM_JEDEC_MX25L3206E.h

@@ -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;
+};
+
+}

libraries/SITL/SIM_SPI.cpp

@@ -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()