From 3b3cc0b194e728063453326d83beadb6371009e5 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sun, 30 May 2021 01:21:33 +0530 Subject: [PATCH] AP_FlashIface: add initial files for AP_FlashIface library --- libraries/AP_FlashIface/AP_FlashIface.h | 22 + .../AP_FlashIface/AP_FlashIface_Abstract.h | 194 ++++ .../AP_FlashIface/AP_FlashIface_JEDEC.cpp | 859 ++++++++++++++++++ libraries/AP_FlashIface/AP_FlashIface_JEDEC.h | 274 ++++++ .../examples/jedec_test/jedec_test.cpp | 157 ++++ .../AP_FlashIface/examples/jedec_test/wscript | 10 + libraries/AP_HAL_ChibiOS/QSPIDevice.cpp | 6 +- 7 files changed, 1519 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_FlashIface/AP_FlashIface.h create mode 100644 libraries/AP_FlashIface/AP_FlashIface_Abstract.h create mode 100644 libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp create mode 100644 libraries/AP_FlashIface/AP_FlashIface_JEDEC.h create mode 100644 libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp create mode 100644 libraries/AP_FlashIface/examples/jedec_test/wscript 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;