diff --git a/libraries/AP_FlashIface/AP_FlashIface.h b/libraries/AP_FlashIface/AP_FlashIface.h
new file mode 100644
index 0000000000..24be8af65f
--- /dev/null
+++ b/libraries/AP_FlashIface/AP_FlashIface.h
@@ -0,0 +1,22 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ *
+ * Code by Siddharth Bharat Purohit
+ */
+/*
+ Implements Common Flash Interface Driver based on
+ Open Standard Published by JEDEC
+*/
+#include "AP_FlashIface_JEDEC.h"
+
diff --git a/libraries/AP_FlashIface/AP_FlashIface_Abstract.h b/libraries/AP_FlashIface/AP_FlashIface_Abstract.h
new file mode 100644
index 0000000000..e793a0601b
--- /dev/null
+++ b/libraries/AP_FlashIface/AP_FlashIface_Abstract.h
@@ -0,0 +1,194 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ *
+ * Code by Siddharth Bharat Purohit
+ */
+/*
+ Implements Common frontend methods for Flash Interface Driver
+*/
+#pragma once
+
+#include
+
+class AP_FlashIface
+{
+
+public:
+ virtual bool init() = 0;
+
+ /**
+ * @details Read data from flash chip.
+ *
+ * @param[in] offset address offset from where to start the read
+ * @param[out] data data to be read from the device
+ * @param[in] size size of the data to be read
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool read(uint32_t offset, uint8_t* data, uint32_t size) = 0;
+
+ /**
+ * @details Gets number bytes that can be written in one go (page size).
+ *
+ * @return page size in bytes.
+ *
+ */
+ virtual uint32_t get_page_size() const = 0;
+
+ /**
+ * @details Gets number pages, each page can written in one go
+ *
+ * @return Number of pages.
+ *
+ */
+ virtual uint32_t get_page_count() const = 0;
+
+ /**
+ * @details Sends command to start programming a page of the chip.
+ *
+ * @param[in] page Page number to be written to
+ * @param[in] data data to be written
+ * @param[out] delay_us Time to wait until next is_device_busy call
+ * @param[out] timeout_us Time after which the erase should be timedout,
+ * should be reset at every call.
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool start_program_page(uint32_t page, const uint8_t *data, uint32_t &delay_ms, uint32_t &timeout_ms) = 0;
+
+ /**
+ * @details Tries to program as much as possible starting from the offset
+ * until size. User needs to call this as many times as needed
+ * taking already programmed bytes into account.
+ *
+ * @param[in] offset address offset for program
+ * @param[in] data data to be programmed
+ * @param[in] size size desired to be programmed
+ * @param[out] programming number of bytes programming, taking care of the limits
+ * @param[out] delay_us Time to wait until program typically finishes
+ * @param[out] timeout_us Time by which current program should have timedout.
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool start_program_offset(uint32_t offset, const uint8_t* data, uint32_t size, uint32_t &programming,
+ uint32_t &delay_ms, uint32_t &timeout_ms)
+ {
+ return false;
+ }
+
+ /**
+ * @details Gets number bytes that can erased in one go(sector size)
+ *
+ * @return Sector size in bytes.
+ *
+ */
+ virtual uint32_t get_sector_size() const = 0;
+
+
+ /**
+ * @details Gets number of sectors, each sector can be erased in one go
+ *
+ * @return Number of sectors.
+ *
+ */
+ virtual uint32_t get_sector_count() const = 0;
+
+ /**
+ * @details Sends command to erase the entire chips.
+ *
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool start_mass_erase(uint32_t &delay_ms, uint32_t &timeout_ms) = 0;
+
+ /**
+ * @details Sends command to erase a sector of the chip.
+ *
+ * @param[in] sector Sector number to be erased
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool start_sector_erase(uint32_t sector, uint32_t &delay_ms, uint32_t &timeout_ms) = 0;
+
+ /**
+ * @details Tries to erase as much as possible starting from the offset
+ * until size. User needs to call this as many times as needed
+ * taking already erased bytes into account, until desired erase
+ * has taken place
+ *
+ * @param[in] offset address offset for erase
+ * @param[in] size size desired to be erased
+ * @param[out] erasing number of bytes erasing
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool start_erase_offset(uint32_t offset, uint32_t size, uint32_t &erasing,
+ uint32_t &delay_ms, uint32_t &timeout_ms)
+ {
+ return false;
+ }
+
+ /**
+ * @details Check if the device is busy.
+ *
+ * @return device busy with last op.
+ *
+ * @retval false if the device is ready.
+ * @retval true if the device is busy.
+ *
+ */
+ virtual bool is_device_busy() = 0;
+
+
+ /**
+ * @details Check if selected sector is erased.
+ *
+ * @param[in] sector sector for which to check erase
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ virtual bool verify_sector_erase(uint32_t sector) = 0;
+
+ /**
+ * @details minimum number of bytes that can be erased
+ *
+ * @return Number of bytes.
+ *
+ */
+ virtual uint32_t min_erase_size() const = 0;
+
+};
diff --git a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp
new file mode 100644
index 0000000000..c670f5fe13
--- /dev/null
+++ b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp
@@ -0,0 +1,859 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ *
+ * Code by Siddharth Bharat Purohit
+ */
+/*
+ Implements Common Flash Interface Driver based on following
+ standard published by JEDEC
+ * JEDEC Standard, JESD216, Serial Flash Discoverable Parameters (SFDP)
+*/
+
+#include
+#include "AP_FlashIface_JEDEC.h"
+#include
+
+extern const AP_HAL::HAL& hal;
+
+struct supported_device {
+ const char* name;
+ uint8_t manufacturer_id;
+ uint8_t device_id;
+};
+
+static const struct supported_device supported_devices[] = {
+ {"mt25q", 0x20, 0xBA}
+};
+
+// Vendor Specific Constants
+// Following Commands Sets were found here:
+// * JEDEC Standard JESD251-A1, Addendum No. 1 to JESD251, Optional x4 Quad I/O
+// With Data Strobe
+/// NOTE: Except Read ID and Multiline Read ID, they seem to be
+// constant across manufacturers, but can't find official standard on
+// this.
+#define CMD_READ_ID 0x9F
+#define CMD_MULTILINE_READ_ID 0xAF
+#define CMD_PAGE_PROGRAM 0x02
+#define CMD_WRITE_DISABLE 0x04
+#define CMD_READ_STATUS 0x05
+#define CMD_MASS_ERASE 0xC7
+#define CMD_RESET_ENABLE 0x66
+#define CMD_RESET_MEMORY 0x99
+#define CMD_READ_SFDP 0x5A
+
+#define SFDP_MASK(lo, hi) (((1UL<<(hi)) - ((1UL<<(lo)))) + (1UL<<(hi)))
+#define SFDP_GET_BITS(x, lo, hi) (((x) & SFDP_MASK(lo, hi)) >> (lo))
+#define SFDP_GET_BIT(x, bit) ((x) & (1<<(bit)))
+
+#define SFDP_HDR_NUM_PARAMS(x) (SFDP_GET_BITS(x[1], 16, 19) + 1)
+#define SFDP_HDR_PARAM_REV(x) SFDP_GET_BITS(x[1], 0, 15)
+#define SFDP_PARAM_ID(x) ((SFDP_GET_BITS(x[0], 0, 3) << 8) | SFDP_GET_BITS(x[1], 24, 31))
+#define SFDP_PARAM_DWORD_LEN(x) SFDP_GET_BITS(x[0], 24, 31)
+#define SFDP_PARAM_POINTER(x) SFDP_GET_BITS(x[1], 0, 23)
+
+#define SFDP_REV_1_6 0x0106
+
+#define Debug(fmt, args ...) do {hal.console->printf("JEDEC: " fmt "\n", ## args);} while(0)
+#define Msg_Print(fmt, args ...) do {hal.console->printf("JEDEC: " fmt "\n", ## args);} while(0)
+
+#define MAX_READ_SIZE 1024UL
+
+bool AP_FlashIface_JEDEC::init()
+{
+ // Get device bus by name
+ _dev = nullptr;
+ for (uint8_t i = 0; i < ARRAY_SIZE(supported_devices); i++) {
+ _dev = hal.qspi->get_device(supported_devices[i].name);
+ if (_dev) {
+ _dev_list_idx = i;
+ break;
+ }
+ }
+ // Reset Device involves trying to soft reset the chip
+ // as when system reboots the device might not have.
+ reset_device();
+
+ // Detecting Device involves trying to read Device ID and matching
+ // with what we expect. Along with extracting info from SFDP
+ if (!detect_device()) {
+ Msg_Print("Failed to detect flash device: %s", supported_devices[_dev_list_idx].name);
+ return false;
+ }
+
+ // Configuring Device involved setting chip to correct WSPI mode
+ // i.e. 4-4-4
+ if (!configure_device()) {
+ Msg_Print("Failed to config flash device: %s", supported_devices[_dev_list_idx].name);
+ return false;
+ }
+
+ Msg_Print("Detected Flash Device: %s", supported_devices[_dev_list_idx].name);
+ return true;
+}
+
+//////////////////////////////////////////////////////
+////////////////// Internal Methods //////////////////
+//////////////////////////////////////////////////////
+
+// reset chip to known default power on state
+void AP_FlashIface_JEDEC::reset_device()
+{
+ // Get chip out of XIP mode
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ uint8_t buf[1];
+ _dev->get_semaphore()->take_blocking();
+
+ /* Attempting a reset of the XIP mode, it could be in an unexpected state
+ because a CPU reset does not reset the memory too.*/
+ /* Resetting XIP mode by reading one byte without XIP confirmation bit.*/
+ cmd.cmd = 0U;
+ cmd.alt = 0xFFU;
+ cmd.addr = 0U;
+ cmd.dummy = 6U;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_NONE |
+ AP_HAL::QSPI::CFG_ADDR_SIZE_24 |
+ AP_HAL::QSPI::CFG_ADDR_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ALT_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ALT_SIZE_8;
+ _dev->set_cmd_header(cmd);
+ _dev->transfer(nullptr, 0, buf, 1);
+
+ /// NOTE: This is Vendor specific, we haven't read the parameter table
+ /// yet, so don't know how we can reset the chip.
+ // Various methods for Soft Reset are at Ref. JESD216 6.4.19
+ /* Quad line CMD_RESET_ENABLE command.*/
+ cmd.cmd = CMD_RESET_ENABLE;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ _dev->transfer(nullptr, 0, nullptr, 0);
+
+ /* Quad line CMD_RESET_MEMORY command.*/
+ cmd.cmd = CMD_RESET_MEMORY;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ _dev->transfer(nullptr, 0, nullptr, 0);
+
+ /* Single line CMD_RESET_MEMORY command.*/
+ cmd.cmd = CMD_RESET_ENABLE;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ _dev->transfer(nullptr, 0, nullptr, 0);
+
+
+ /* Single line N25Q_CMD_RESET_MEMORY command.*/
+ cmd.cmd = CMD_RESET_MEMORY;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ _dev->transfer(nullptr, 0, nullptr, 0);
+
+ // By now we are pretty sure the chip is reset
+}
+
+// Does initial configuration to bring up and setup chip
+bool AP_FlashIface_JEDEC::detect_device()
+{
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+
+ {
+ uint8_t buf[3] {};
+ cmd.cmd = CMD_READ_ID;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE |
+ AP_HAL::QSPI::CFG_DATA_MODE_ONE_LINE;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, buf, sizeof(buf))) {
+ Debug("Failed to read Device ID");
+ return false;
+ }
+
+ if (buf[0] != supported_devices[_dev_list_idx].manufacturer_id ||
+ buf[1] != supported_devices[_dev_list_idx].device_id) {
+ return false;
+ }
+ }
+
+ // Read SFDP header to get information Ref. JESD216 4 and 6.2
+ {
+ uint32_t sfdp_header[2];
+
+ cmd.cmd = CMD_READ_SFDP;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE |
+ AP_HAL::QSPI::CFG_ADDR_MODE_ONE_LINE |
+ AP_HAL::QSPI::CFG_ADDR_SIZE_24 |
+ AP_HAL::QSPI::CFG_DATA_MODE_ONE_LINE;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 8; // 8 dummy cycles
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, (uint8_t*)sfdp_header, sizeof(sfdp_header))) {
+ Debug("SFDP Header read failed");
+ return false;
+ }
+
+ // Read Signature
+ if (memcmp(sfdp_header, "SFDP", 4)) {
+ Debug("SFDP Bad Signature: 0x%lx", (unsigned long)sfdp_header[0]);
+ return false;
+ }
+
+ // Read Num Param Headers
+ if (SFDP_HDR_NUM_PARAMS(sfdp_header) == 0) {
+ Debug("Unsupported number of param headers %ld", (unsigned long)SFDP_HDR_NUM_PARAMS(sfdp_header));
+ return false;
+ }
+ // Read Revision
+ _desc.param_rev = SFDP_HDR_PARAM_REV(sfdp_header);
+ if (_desc.param_rev != SFDP_REV_1_6) {
+ Debug("Unsupported revision %x", (unsigned int)_desc.param_rev);
+ return false;
+ }
+ }
+
+
+ // Read Param Header Ref. JESD216 6.4.1 6.4.2
+ {
+ uint32_t param_header[2] {}; // read only first parameter header
+ // Immediately after 2 DWORDS of SFDP Header
+ cmd.addr = 2*sizeof(uint32_t);
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, (uint8_t*)param_header, sizeof(param_header))) {
+ Debug("Param header read failed");
+ return false;
+ }
+
+ if (SFDP_PARAM_ID(param_header) != 0xFF) {
+ Debug("Only basic Param Table supported not %lx", (unsigned long)SFDP_PARAM_ID(param_header));
+ return false;
+ }
+ // Lets get the length of parameter table
+ _desc.param_table_len = MIN(SFDP_PARAM_DWORD_LEN(param_header), 20UL);
+ _desc.param_table_pointer = SFDP_PARAM_POINTER(param_header);
+ }
+
+ // Read and parse the param table
+ {
+ uint32_t param_table[20] {};
+ cmd.addr = _desc.param_table_pointer;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, (uint8_t*)param_table, _desc.param_table_len*sizeof(uint32_t))) {
+ Debug("Failed to read Parameter Table");
+ return false;
+ }
+
+ // Flash Memory details Ref. JESD216 6.4.5 6.4.14
+ if (SFDP_GET_BIT(param_table[1], 31)) {
+ Debug("Unsupported Flash Size");
+ return false;
+ }
+ _desc.flash_size = SFDP_GET_BITS(param_table[1], 0, 30)/8;
+ _desc.page_size = 1UL< 0) {
+ _desc.erase_type[i].size = size;
+ _desc.erase_type[i].ins = ins;
+ if (size > _desc.sector_size) {
+ _desc.sector_size = size;
+ }
+ if (size < _desc.min_erase_size) {
+ _desc.min_erase_size = size;
+ }
+ }
+ }
+ _desc.sector_count = _desc.flash_size/_desc.sector_size;
+ if (_desc.sector_count == 0) {
+ _desc.sector_count = 1;
+ }
+ // Read Erase Times 6.4.13
+ uint8_t timeout_mult = 2*(SFDP_GET_BITS(param_table[9], 0, 3) + 1);
+ for (uint8_t i = 0; i < 4; i++) {
+ if (_desc.erase_type[i].size) {
+ uint32_t unit = SFDP_GET_BITS(param_table[9], 9+(7*i), 10+(7*i));
+ uint8_t val = SFDP_GET_BITS(param_table[9], 4+(7*i), 8+(7*i));
+ if (unit == 0b00) {
+ unit = 1; //1ms
+ } else if (unit == 0b01) {
+ unit = 16; // 16ms
+ } else if (unit == 0b10) {
+ unit = 128; // 128ms
+ } else if (unit == 0b11) {
+ unit = 1000; // 1s
+ }
+
+ _desc.erase_type[i].delay_ms = (val+1)*unit;
+ _desc.erase_type[i].timeout_ms = timeout_mult*_desc.erase_type[i].delay_ms;
+ }
+ }
+ // Mass Erase times 6.4.14
+ uint32_t unit = SFDP_GET_BITS(param_table[10], 29, 30);
+ if (unit == 0b00) {
+ unit = 16; // 16ms
+ } else if (unit == 0b01) {
+ unit = 256; // 256ms
+ } else if (unit == 0b10) {
+ unit = 4000; // 4s
+ } else if (unit == 0b11) {
+ unit = 64000; // 64s
+ }
+ _desc.mass_erase_delay_ms = (SFDP_GET_BITS(param_table[10], 24, 28) + 1)*unit;
+ _desc.mass_erase_timeout_ms = timeout_mult*_desc.mass_erase_delay_ms;
+
+ // Setup Write Enable Instruction Ref. JESD216 6.4.19
+ // If needed legacy support Ref. JESD216 6.4.4 and implement that
+ if (SFDP_GET_BIT(param_table[15], 0) ||
+ SFDP_GET_BIT(param_table[15], 1)) {
+ _desc.write_enable_ins = 0x06;
+ } else if (SFDP_GET_BIT(param_table[15], 2)) {
+ _desc.write_enable_ins = 0x50;
+ } else if (SFDP_GET_BITS(param_table[15], 3, 6)) {
+ Debug("Unsupported Register Write Enable Config");
+ return false;
+ }
+
+ // Setup Program timings Ref. JESD216 6.4.14
+ // unit = SFDP_GET_BIT(param_table[10], 23)?1:8;
+ // _desc.add_byte_prog_delay_us = (SFDP_GET_BITS(19, 22) + 1) * unit;
+ // _desc.add_byte_prog_timeout_us = _desc.add_byte_prog_delay_us * timeout_mult;
+ // unit = SFDP_GET_BIT(param_table[10], 18)?1:8;
+ // _desc.first_byte_prog_delay_us = (SFDP_GET_BITS(14, 17) + 1) * unit;
+ // _desc.first_byte_prog_timeout_us = _desc.first_byte_prog_delay_us * timeout_mult;
+
+ // Implement above code if more precise delay and timeouts are needed while programming
+ // otherwise fraction of page timings should be fine
+ timeout_mult = 2*(SFDP_GET_BITS(param_table[10], 0, 3) + 1);
+ unit = SFDP_GET_BIT(param_table[10], 18)?8:64;
+ _desc.page_prog_delay_us = (SFDP_GET_BITS(param_table[10], 8, 13) + 1) * unit;
+ _desc.page_prog_timeout_us = _desc.page_prog_delay_us * timeout_mult;
+
+
+ // Configure Quad Mode Enable and Read Sequence, Ref. JESD216 6.4.8 6.4.10 6.4.18
+ if (!SFDP_GET_BIT(param_table[4], 4)) {
+ Debug("Quad mode unsupported");
+ return false;
+ }
+
+ _desc.fast_read_ins = SFDP_GET_BITS(param_table[6], 24, 31);
+ // we get number of dummy clocks cycles needed, also include mode bits
+ _desc.fast_read_dummy_cycles = SFDP_GET_BITS(param_table[6], 16, 20) +
+ SFDP_GET_BITS(param_table[6], 21, 23);
+
+ uint8_t QER = SFDP_GET_BITS(param_table[14], 20, 22);
+ if (QER != 0b000) {
+ Debug("Unsupported Quad Enable Requirement");
+ return false;
+ }
+ if (SFDP_GET_BIT(param_table[14], 4)) {
+ Debug("Unsupported Quad Enable Requirement: set QE bits");
+ return false;
+ }
+
+ if (SFDP_GET_BIT(param_table[14], 5)) {
+ _desc.quad_mode_ins = 0x38;
+ } else if (SFDP_GET_BIT(param_table[14], 6)) {
+ _desc.quad_mode_ins = 0x35;
+ }
+
+ if (SFDP_GET_BIT(param_table[14], 7)) {
+ Debug("Unsupported Quad enable seq");
+ return false;
+ } else if (SFDP_GET_BIT(param_table[14], 8)) {
+ _desc.quad_mode_rmw_seq = true;
+ } else {
+ _desc.quad_mode_rmw_seq = false;
+ }
+
+ // Configure Status Polling Method Ref. JESD216 6.4.17
+ if (SFDP_GET_BIT(param_table[13], 3)) {
+ _desc.legacy_status_polling = false;
+ _desc.status_read_ins = 0x70;
+ } else if (SFDP_GET_BIT(param_table[13], 2)) {
+ _desc.legacy_status_polling = true;
+ _desc.status_read_ins = 0x05;
+ }
+ }
+ initialised = true;
+ return true;
+}
+
+// Configures device to normal working state, currently 4-4-4 QSPI
+bool AP_FlashIface_JEDEC::configure_device()
+{
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ // Enable 4-4-4 mode and test it by fetching Device ID
+ {
+ // Quad Mode Enable Sequence, Ref. JESD216 6.4.18
+ if (_desc.quad_mode_ins && !write_enable_called) {
+ if (!send_cmd(_desc.quad_mode_ins, false)) {
+ return false;
+ }
+ } else if (_desc.quad_mode_rmw_seq) {
+ write_enable(false);
+ if (!modify_reg(0x65, 0x61, (1<<7), 0, false)) {
+ write_disable(false);
+ return false;
+ }
+ write_disable(true);
+ }
+
+ uint8_t buf[3] {};
+ cmd.cmd = CMD_MULTILINE_READ_ID;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, buf, sizeof(buf))) {
+ Debug("Failed to switch to Quad mode");
+ return false;
+ }
+
+ if (buf[0] != supported_devices[_dev_list_idx].manufacturer_id ||
+ buf[1] != supported_devices[_dev_list_idx].device_id) {
+ Debug("Device bad mfg_id(0x%x) and dev_id(0x%x)", (unsigned int)buf[0], (unsigned int)buf[1]);
+ return false;
+ }
+ Debug("Device configured for 4-4-4 mode");
+ }
+ // Hurray! We are in 4-4-4 mode
+ _quad_spi_mode = true;
+ return true;
+}
+
+// Enables commands that modify flash data or settings
+bool AP_FlashIface_JEDEC::write_enable(bool quad_mode)
+{
+ if (_desc.write_enable_ins) {
+ write_enable_called = true;
+ return send_cmd(_desc.write_enable_ins, quad_mode);
+ }
+ return true;
+}
+
+// Disables commands that modify flash data or settings
+bool AP_FlashIface_JEDEC::write_disable(bool quad_mode)
+{
+ if (_desc.write_enable_ins) {
+ write_enable_called = true;
+ return send_cmd(CMD_WRITE_DISABLE, quad_mode);
+ }
+ return true;
+}
+
+// Read modify write register
+bool AP_FlashIface_JEDEC::modify_reg(uint8_t read_ins, uint8_t write_ins,
+ uint8_t mask, uint8_t val, bool quad_mode)
+{
+ // Read
+ uint8_t reg_val;
+ if (!read_reg(read_ins, reg_val, quad_mode)) {
+ return false;
+ }
+
+ // Modify
+ reg_val = (reg_val & ~mask) | (val & mask);
+
+ // Write
+ if (!write_reg(write_ins, reg_val, quad_mode)) {
+ return false;
+ }
+ return true;
+}
+
+// reads a register value of chip using instruction
+bool AP_FlashIface_JEDEC::read_reg(uint8_t read_ins, uint8_t &read_val, bool quad_mode)
+{
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = read_ins;
+ if (quad_mode) {
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES;
+ } else {
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE |
+ AP_HAL::QSPI::CFG_DATA_MODE_ONE_LINE;
+ }
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, &read_val, sizeof(read_val))) {
+ Debug("Failed Register Read");
+ return false;
+ }
+ return true;
+}
+
+// sends instruction to write a register value in the chip
+bool AP_FlashIface_JEDEC::write_reg(uint8_t read_ins, uint8_t write_val, bool quad_mode)
+{
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = read_ins;
+ if (quad_mode) {
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES;
+ } else {
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE |
+ AP_HAL::QSPI::CFG_DATA_MODE_ONE_LINE;
+ }
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(&write_val, 1, nullptr, 0)) {
+ Debug("Failed Register Write");
+ return false;
+ }
+ return true;
+}
+
+// Sends QSPI command without data
+bool AP_FlashIface_JEDEC::send_cmd(uint8_t ins, bool quad_mode)
+{
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = ins;
+ if (quad_mode) {
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES;
+ } else {
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE;
+ }
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, nullptr, 0)) {
+ Debug("Failed Register Write");
+ return false;
+ }
+ return true;
+}
+
+//////////////////////////////////////////////////////
+////////////////////PUBLIC METHODS////////////////////
+//////////////////////////////////////////////////////
+/**
+ * @details Sends command to erase the entire chips.
+ *
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::start_mass_erase(uint32_t &delay_ms, uint32_t &timeout_ms)
+{
+ write_enable(true);
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = CMD_MASS_ERASE;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES;
+ cmd.addr = 0;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, nullptr, 0)) { // Command only
+ write_disable(true);
+ Debug("Failed to send erase command");
+ return false;
+ }
+ delay_ms = _desc.mass_erase_delay_ms;
+ timeout_ms = _desc.mass_erase_timeout_ms;
+ write_disable(true);
+ return true;
+}
+
+/**
+ * @details Sends command to erase a sector of the chip.
+ *
+ * @param[in] sector Sector number to be erased
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::start_sector_erase(uint32_t sector, uint32_t &delay_ms, uint32_t &timeout_ms)
+{
+ if (sector > _desc.sector_count) {
+ Debug("Invalid sector");
+ return false;
+ }
+ uint32_t erasing;
+ bool ret = start_erase_offset(_desc.sector_size*sector, _desc.sector_size, erasing, delay_ms, timeout_ms);
+ if (!ret || (erasing != _desc.sector_size)) {
+ Debug("Failed to erase sector");
+ return false;
+ }
+ return true;
+}
+
+/**
+ * @details Tries to erase as much as possible starting from the offset
+ * until size. User needs to call this as many times as needed
+ * taking already erased bytes into account, until desired erase
+ * has taken place
+ *
+ * @param[in] offset address offset for erase
+ * @param[in] size size desired to be erased
+ * @param[out] erasing number of bytes erasing
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::start_erase_offset(uint32_t offset, uint32_t size, uint32_t &erasing,
+ uint32_t &delay_ms, uint32_t &timeout_ms)
+{
+ uint8_t ins;
+ uint32_t erase_size = 0;
+ erasing = 0;
+ // Find the maximum size we can erase
+ for (uint8_t i=0; i < 4; i++) {
+ if (_desc.erase_type[i].size == 0) {
+ continue;
+ }
+ if (_desc.erase_type[i].size < erase_size) {
+ // we already found a larger size we can erase
+ continue;
+ }
+ // check if we can find an instruction to match the erase req.
+ if ((size >= _desc.erase_type[i].size) && !(offset % _desc.erase_type[i].size)) {
+ erase_size = size;
+ ins = _desc.erase_type[i].ins;
+ delay_ms = _desc.erase_type[i].delay_ms;
+ timeout_ms = _desc.erase_type[i].timeout_ms;
+ }
+ }
+ if (erase_size == 0) {
+ Debug("Requested Erase size is too small");
+ return false;
+ }
+ // Start Erasing
+ write_enable(true);
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = ins;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ADDR_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ADDR_SIZE_24;
+ cmd.addr = offset;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, nullptr, 0)) { // Command only
+ write_disable(true);
+ Debug("Failed to send erase command");
+ return false;
+ }
+ write_disable(true);
+ erasing = erase_size;
+ return true;
+}
+
+
+/**
+ * @details Check if selected sector is erased.
+ *
+ * @param[in] sector sector for which to check erase
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::verify_sector_erase(uint32_t sector)
+{
+ uint8_t buf[MAX_READ_SIZE] {}; // Read 1KB per read
+ for (uint32_t offset = _desc.sector_size*sector; offset < (_desc.sector_size*(sector+1)); offset+=sizeof(buf)) {
+ if (read(offset, buf, sizeof(buf))) {
+ for (uint16_t i = 0; i < sizeof(buf); i++) {
+ if (buf[i] != 0xFF) {
+ Debug("Found unerased byte %x @ offset %ld", (unsigned int)buf[i], (unsigned long)offset);
+ return false;
+ }
+ }
+ } else {
+ Debug("Read Failed");
+ return false;
+ }
+ }
+ return true;
+}
+
+/**
+ * @details Sends command to start programming a page of the chip.
+ *
+ * @param[in] page Page number to be written to
+ * @param[in] data data to be written
+ * @param[out] delay_us Time to wait until next is_device_busy call
+ * @param[out] timeout_us Time after which the erase should be timedout,
+ * should be reset at every call.
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::start_program_page(uint32_t page, const uint8_t* data,
+ uint32_t &delay_us, uint32_t &timeout_us)
+{
+ if (page > _desc.page_count) {
+ Debug("Invalid Page");
+ return false;
+ }
+ uint32_t programming;
+ bool ret = start_program_offset(_desc.page_size*page, data, _desc.sector_size, programming, delay_us, timeout_us);
+ if (!ret || (programming != _desc.page_size)) {
+ Debug("Failed to program page");
+ return false;
+ }
+ return true;
+}
+
+/**
+ * @details Tries to program as much as possible starting from the offset
+ * until size. User needs to call this as many times as needed
+ * taking already programmed bytes into account.
+ *
+ * @param[in] offset address offset for program
+ * @param[in] data data to be programmed
+ * @param[in] size size desired to be programmed
+ * @param[out] programming number of bytes programming, taking care of the limits
+ * @param[out] delay_us Time to wait until program typically finishes
+ * @param[out] timeout_us Time by which current program should have timedout.
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::start_program_offset(uint32_t offset, const uint8_t* data, uint32_t size, uint32_t &programming,
+ uint32_t &delay_us, uint32_t &timeout_us)
+{
+ if (size > _desc.page_size) {
+ // we can only program single
+ // page at the max in one go
+ size = _desc.page_size;
+ }
+ // Ensure we don't go beyond the page of offset, while writing
+ size = MIN(_desc.page_size - (offset % _desc.page_size), size);
+
+ write_enable(true);
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = CMD_PAGE_PROGRAM;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ADDR_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ADDR_SIZE_24 |
+ AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES;
+ cmd.addr = offset;
+ cmd.alt = 0;
+ cmd.dummy = 0;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(data, size, nullptr, 0)) { // Command only
+ write_disable(true);
+ Debug("Failed to send program command");
+ return false;
+ }
+ write_disable(true);
+ programming = size;
+ // we are mostly going to program in chunks so this will do
+ delay_us = (_desc.page_prog_delay_us*size)/(_desc.page_size);
+ timeout_us = (_desc.page_prog_timeout_us*size)/(_desc.page_size);
+ return true;
+}
+
+/**
+ * @details Read data from flash chip.
+ *
+ * @param[in] offset address offset from where to start the read
+ * @param[out] data data to be read from the device
+ * @param[in] size size of the data to be read
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+bool AP_FlashIface_JEDEC::read(uint32_t offset, uint8_t* data, uint32_t size)
+{
+ if ((offset + size) > _desc.flash_size) {
+ // reading more than what exists
+ return false;
+ }
+ uint32_t read_ptr, read_size;
+ for (read_ptr = offset; read_ptr < (offset+size); read_ptr+=MAX_READ_SIZE) {
+ read_size = MIN((offset+size) - read_ptr, MAX_READ_SIZE);
+ AP_HAL::QSPIDevice::CommandHeader cmd;
+ cmd.cmd = _desc.fast_read_ins;
+ cmd.cfg = AP_HAL::QSPI::CFG_CMD_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ADDR_MODE_FOUR_LINES |
+ AP_HAL::QSPI::CFG_ADDR_SIZE_24 |
+ AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES;
+ cmd.addr = read_ptr;
+ cmd.alt = 0;
+ cmd.dummy = _desc.fast_read_dummy_cycles;
+ _dev->set_cmd_header(cmd);
+ if (!_dev->transfer(nullptr, 0, &data[read_ptr-offset], read_size)) { // Command only
+ Debug("Failed to read flash");
+ return false;
+ }
+ }
+ return true;
+}
+
+/**
+ * @details Check if the device is busy.
+ *
+ * @return device busy with last op.
+ *
+ * @retval false if the device is ready.
+ * @retval true if the device is busy.
+ *
+ */
+bool AP_FlashIface_JEDEC::is_device_busy()
+{
+ uint8_t status;
+ read_reg(_desc.status_read_ins, status, true);
+ if (_desc.legacy_status_polling) {
+ return (status & 0x1);
+ } else {
+ return !(status & 1<<7);
+ }
+}
diff --git a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.h b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.h
new file mode 100644
index 0000000000..1475c6a9e8
--- /dev/null
+++ b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.h
@@ -0,0 +1,274 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ *
+ * Code by Siddharth Bharat Purohit
+ */
+/*
+ Implements Common Flash Interface Driver based on
+ Open Standard Published by JEDEC
+*/
+
+#include "AP_FlashIface_Abstract.h"
+
+
+class AP_FlashIface_JEDEC : public AP_FlashIface
+{
+public:
+ bool init() override;
+
+ /**
+ * @details Read data from flash chip.
+ *
+ * @param[in] offset address offset from where to start the read
+ * @param[out] data data to be read from the device
+ * @param[in] size size of the data to be read
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool read(uint32_t offset, uint8_t *data, uint32_t size) override;
+
+
+ /**
+ * @details Gets number bytes that can be written in one go (page size).
+ *
+ * @return page size in bytes.
+ *
+ */
+ uint32_t get_page_size() const override
+ {
+ return _desc.page_size;
+ }
+
+ /**
+ * @details Gets number pages, each page can written in one go
+ *
+ * @return Number of pages.
+ *
+ */
+ uint32_t get_page_count() const override
+ {
+ return _desc.page_count;
+ }
+
+ /**
+ * @details Sends command to start programming a page of the chip.
+ *
+ * @param[in] page Page number to be written to
+ * @param[in] data data to be written
+ * @param[out] delay_us Time to wait until next is_device_busy call
+ * @param[out] timeout_us Time after which the erase should be timedout,
+ * should be reset at every call.
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool start_program_page(uint32_t page, const uint8_t *data, uint32_t &delay_us, uint32_t &timeout_us) override;
+
+ /**
+ * @details Tries to program as much as possible starting from the offset
+ * until size. User needs to call this as many times as needed
+ * taking already programmed bytes into account.
+ *
+ * @param[in] offset address offset for program
+ * @param[in] data data to be programmed
+ * @param[in] size size desired to be programmed
+ * @param[out] programming number of bytes programming, taking care of the limits
+ * @param[out] delay_us Time to wait until program typically finishes
+ * @param[out] timeout_us Time by which current program should have timedout.
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool start_program_offset(uint32_t offset, const uint8_t* data, uint32_t size, uint32_t &programming,
+ uint32_t &delay_us, uint32_t &timeout_us) override;
+
+ // Erase Methods
+
+ /**
+ * @details Sends command to erase the entire chips.
+ *
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool start_mass_erase(uint32_t &delay_ms, uint32_t &timeout_ms) override;
+
+ /**
+ * @details Gets number bytes that can erased in one go(sector size)
+ *
+ * @return Sector size in bytes.
+ *
+ */
+ uint32_t get_sector_size() const override
+ {
+ return _desc.sector_size;
+ }
+
+ /**
+ * @details Gets number of sectors, each sector can be erased in one go
+ *
+ * @return Number of sectors.
+ *
+ */
+ uint32_t get_sector_count() const override
+ {
+ return _desc.sector_count;
+ }
+
+ /**
+ * @details minimum number of bytes that can be erased
+ *
+ * @return Number of bytes.
+ *
+ */
+ uint32_t min_erase_size() const override
+ {
+ return _desc.min_erase_size;
+ }
+
+
+ /**
+ * @details Sends command to erase a sector of the chip.
+ *
+ * @param[in] sector Sector number to be erased
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool start_sector_erase(uint32_t sector, uint32_t &delay_ms, uint32_t &timeout_ms) override;
+
+ /**
+ * @details Tries to erase as much as possible starting from the offset
+ * until size. User needs to call this as many times as needed
+ * taking already erased bytes into account, until desired erase
+ * has taken place
+ *
+ * @param[in] offset address offset for erase
+ * @param[in] size size desired to be erased
+ * @param[out] erasing number of bytes erasing
+ * @param[out] delay_ms Time to wait until next is_device_busy call
+ * @param[out] timeout_ms Time by which the erase should have timedout
+ *
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool start_erase_offset(uint32_t offset, uint32_t size, uint32_t &erasing,
+ uint32_t &delay_ms, uint32_t &timeout_ms) override;
+
+
+ /**
+ * @details Check if selected sector is erased.
+ *
+ * @param[in] sector sector for which to check erase
+ * @return The operation status.
+ * @retval false if the operation failed.
+ * @retval true if the operation succeeded.
+ *
+ */
+ bool verify_sector_erase(uint32_t sector) override;
+
+
+ /**
+ * @details Check if the device is busy.
+ *
+ * @return device busy with last op.
+ *
+ * @retval false if the device is ready.
+ * @retval true if the device is busy.
+ *
+ */
+ bool is_device_busy() override;
+
+protected:
+ void reset_device();
+
+ // Does initial configuration to bring up and setup chip
+ bool detect_device();
+
+ // Configures device to normal working state, currently 4-4-4 QSPI
+ bool configure_device();
+
+ // Enables commands that modify flash data or settings
+ bool write_enable(bool quad_mode);
+
+ // Disables commands that modify flash data or settings
+ bool write_disable(bool quad_mode);
+
+ // Read modify write register
+ bool modify_reg(uint8_t read_ins, uint8_t write_ins,
+ uint8_t mask, uint8_t val, bool quad_mode);
+
+ // reads a register value of chip using instruction
+ bool read_reg(uint8_t read_ins, uint8_t &read_val, bool quad_mode);
+
+ // sends instruction to write a register value in the chip
+ bool write_reg(uint8_t read_ins, uint8_t write_val, bool quad_mode);
+
+ // Sends QSPI command without data
+ bool send_cmd(uint8_t ins, bool quad_mode);
+
+ // Is device in quad spi mode
+ bool _quad_spi_mode;
+
+ AP_HAL::OwnPtr _dev;
+
+ // Device description extracted from SFDP
+ struct device_desc {
+ uint16_t param_rev; //parameter revision
+ uint8_t param_table_len; // size of parameter table
+ uint32_t param_table_pointer; // location of parameter table
+ uint32_t flash_size; // size of flash in bytes
+ uint32_t page_size; // maximum size that can be written in one transaction
+ uint32_t page_count; // number of pages each of page size
+ uint32_t sector_size; // maximum number of bytes that can be erased outside of mass erase
+ uint32_t sector_count; // number of sectors
+ uint32_t min_erase_size; // minimum amount of bytes that can be erased
+ struct {
+ uint8_t ins; // instruction for the erase
+ uint32_t size; // number of bytes that will be erased
+ uint32_t delay_ms; // typical time this command will finish
+ uint32_t timeout_ms; // time after which the erase cmd caller should time
+ } erase_type[4];
+ uint32_t mass_erase_delay_ms; // typical time taken while mass erase
+ uint32_t mass_erase_timeout_ms; // time after which mass erase cmd caller should timeout
+ uint8_t write_enable_ins; // instruction to allow enabling modification of register and data
+ uint32_t page_prog_delay_us; // time taken to write a page worth of data to flash
+ uint32_t page_prog_timeout_us; // time after which the page program caller should timeout
+ uint8_t fast_read_ins; // instruction to do fast read, i.e. read any number of bytes in single trx
+ uint8_t fast_read_dummy_cycles; // number of dummy cycles after which the chip will respond with data
+ uint8_t quad_mode_ins; // instruction to enter 4-4-4 mode
+ bool quad_mode_rmw_seq; // use Read modify write sequence to enter 4-4-4 mode supported or not
+ uint8_t status_read_ins; // read status of the chip, gets us if busy writing/erasing
+ bool legacy_status_polling; // check if legacy status polling supported or not
+ } _desc;
+
+ uint8_t _dev_list_idx;
+ bool initialised;
+ bool write_enable_called;
+};
+
diff --git a/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp b/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp
new file mode 100644
index 0000000000..4bcba5cf2b
--- /dev/null
+++ b/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp
@@ -0,0 +1,157 @@
+#include
+#include
+#include
+#include
+
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+AP_FlashIface_JEDEC jedec_dev;
+
+void setup();
+void loop();
+
+static AP_BoardConfig board_config;
+
+static UNUSED_FUNCTION void test_page_program()
+{
+ uint8_t *data = new uint8_t[jedec_dev.get_page_size()];
+ if (data == nullptr) {
+ hal.console->printf("Failed to allocate data for program");
+ }
+ uint8_t *rdata = new uint8_t[jedec_dev.get_page_size()];
+ if (rdata == nullptr) {
+ hal.console->printf("Failed to allocate data for read");
+ }
+
+ // fill program data with its own adress
+ for (uint32_t i = 0; i < jedec_dev.get_page_size(); i++) {
+ data[i] = i;
+ }
+ hal.console->printf("Writing Page #1\n");
+ uint32_t delay_us, timeout_us;
+ uint64_t start_time_us = AP_HAL::micros64();
+ if (!jedec_dev.start_program_page(0, data, delay_us, timeout_us)) {
+ hal.console->printf("Page write command failed\n");
+ return;
+ }
+ while (true) {
+ hal.scheduler->delay_microseconds(delay_us);
+ if (AP_HAL::micros64() > (start_time_us+delay_us)) {
+ if (!jedec_dev.is_device_busy()) {
+ hal.console->printf("Page Program Successful, elapsed %lld us\n", AP_HAL::micros64() - start_time_us);
+ break;
+ } else {
+ hal.console->printf("Typical page program time reached, Still Busy?!\n");
+ }
+ }
+ if (AP_HAL::micros64() > (start_time_us+timeout_us)) {
+ hal.console->printf("Page Program Timed out, elapsed %lld us\n", AP_HAL::micros64() - start_time_us);
+ return;
+ }
+ }
+ if (!jedec_dev.read(0, rdata, jedec_dev.get_page_size())) {
+ hal.console->printf("Failed to read Flash page\n");
+ } else {
+ if (memcmp(data, rdata, jedec_dev.get_page_size()) != 0) {
+ hal.console->printf("Program Data Mismatch!\n");
+ } else {
+ hal.console->printf("Program Data Verified Good!\n");
+ }
+ }
+}
+
+static UNUSED_FUNCTION void test_sector_erase()
+{
+ uint32_t delay_ms, timeout_ms;
+ if (!jedec_dev.start_sector_erase(0, delay_ms, timeout_ms)) { // erase first sector
+ hal.console->printf("Sector erase command failed\n");
+ return;
+ }
+ uint32_t erase_start = AP_HAL::millis();
+ uint32_t next_check_ms = 0;
+ hal.console->printf("Erasing Sector #1 ");
+ while (true) {
+ if (AP_HAL::millis() > next_check_ms) {
+ hal.console->printf("\n");
+ if (!jedec_dev.is_device_busy()) {
+ if (next_check_ms == 0) {
+ hal.console->printf("Sector Erase happened too fast\n");
+ return;
+ }
+ hal.console->printf("Sector Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
+ break;
+ } else {
+ hal.console->printf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
+ }
+ if ((AP_HAL::millis() - erase_start) > timeout_ms) {
+ hal.console->printf("Sector Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
+ return;
+ }
+ next_check_ms = erase_start+delay_ms;
+ }
+ hal.scheduler->delay((delay_ms/100) + 10);
+ hal.console->printf("*");
+ }
+ if (!jedec_dev.verify_sector_erase(0)) {
+ hal.console->printf("Erase Verification Failed!\n");
+ } else {
+ hal.console->printf("Erase Verification Successful!\n");
+ }
+}
+
+static UNUSED_FUNCTION void test_mass_erase()
+{
+ uint32_t delay_ms, timeout_ms;
+ if (!jedec_dev.start_mass_erase(delay_ms, timeout_ms)) {
+ hal.console->printf("Mass erase command failed\n");
+ return;
+ }
+ uint32_t erase_start = AP_HAL::millis();
+ uint32_t next_check_ms = 0;
+ hal.console->printf("Mass Erasing ");
+ while (true) {
+ if (AP_HAL::millis() > next_check_ms) {
+ hal.console->printf("\n");
+ if (!jedec_dev.is_device_busy()) {
+ if (next_check_ms == 0) {
+ hal.console->printf("Sector Erase happened too fast\n");
+ return;
+ }
+ hal.console->printf("Mass Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
+ return;
+ } else {
+ hal.console->printf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
+ }
+ if ((AP_HAL::millis() - erase_start) > timeout_ms) {
+ hal.console->printf("Mass Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
+ return;
+ }
+ next_check_ms = erase_start+delay_ms;
+ }
+ hal.scheduler->delay(delay_ms/100);
+ hal.console->printf("*");
+ }
+}
+
+
+void setup()
+{
+ board_config.init();
+}
+
+void loop()
+{
+ // Start on user input
+ while (!hal.console->available()) {
+ hal.scheduler->delay(100);
+ }
+ hal.console->printf("\n\n******************Starting Test********************\n");
+ jedec_dev.init();
+ test_page_program();
+ test_sector_erase();
+ // test_mass_erase();
+ hal.scheduler->delay(1000);
+}
+
+AP_HAL_MAIN();
diff --git a/libraries/AP_FlashIface/examples/jedec_test/wscript b/libraries/AP_FlashIface/examples/jedec_test/wscript
new file mode 100644
index 0000000000..61c0caa07c
--- /dev/null
+++ b/libraries/AP_FlashIface/examples/jedec_test/wscript
@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+ bld.ap_example(
+ use=['ap','JEDEC_libs'],
+ )
+ bld.ap_stlib(name= 'JEDEC_libs',
+ ap_vehicle='AP_Periph',
+ ap_libraries= ['AP_FlashIface'])
diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp
index d3ee593f5c..3aa964effe 100644
--- a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp
@@ -79,14 +79,14 @@ bool QSPIDevice::transfer(const uint8_t *send, uint32_t send_len,
if (send_len == 0 && recv_len == 0) {
// This is just a command
- ret = wspiCommand(qspi_devices[device_desc.bus].driver, &mode);
+ ret = !wspiCommand(qspi_devices[device_desc.bus].driver, &mode);
} else if (send_len > 0 && recv == 0) {
// This is a send cmd
- ret = wspiSend(qspi_devices[device_desc.bus].driver, &mode, send_len, send);
+ ret = !wspiSend(qspi_devices[device_desc.bus].driver, &mode, send_len, send);
} else if (send_len == 0 && recv_len >= 1) {
// This is a receive cmd,
// we only consume first byte of send
- ret = wspiReceive(qspi_devices[device_desc.bus].driver, &mode, recv_len, recv);
+ ret = !wspiReceive(qspi_devices[device_desc.bus].driver, &mode, recv_len, recv);
} else {
// Can't handle this transaction type
ret = false;