From ae6872c6f6b82c54aa009604d30c78c9f4d7f1e2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 30 May 2022 22:22:31 +0100 Subject: [PATCH] AP_Logger: support W25N01GV flash chips add separate driver for W25N01GV triggered via HAL_LOGGING_DATAFLASH_DRIVER move flash_test() into AP_Logger_Block. cleanup use of 4k sector commands to account for chips that only have block commands --- libraries/AP_Logger/AP_Logger.cpp | 9 +- libraries/AP_Logger/AP_Logger_Block.cpp | 67 +++- libraries/AP_Logger/AP_Logger_Block.h | 2 +- libraries/AP_Logger/AP_Logger_DataFlash.cpp | 30 +- libraries/AP_Logger/AP_Logger_DataFlash.h | 1 - libraries/AP_Logger/AP_Logger_W25N01GV.cpp | 319 ++++++++++++++++++++ libraries/AP_Logger/AP_Logger_W25N01GV.h | 47 +++ 7 files changed, 433 insertions(+), 42 deletions(-) create mode 100644 libraries/AP_Logger/AP_Logger_W25N01GV.cpp create mode 100644 libraries/AP_Logger/AP_Logger_W25N01GV.h diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 0b2a0ced1a..688240bc7d 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -6,6 +6,7 @@ #include "AP_Logger_File.h" #include "AP_Logger_DataFlash.h" +#include "AP_Logger_W25N01GV.h" #include "AP_Logger_MAVLink.h" #include @@ -28,6 +29,10 @@ extern const AP_HAL::HAL& hal; #endif #endif +#ifndef HAL_LOGGING_DATAFLASH_DRIVER +#define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_DataFlash +#endif + #ifndef HAL_LOGGING_STACK_SIZE #define HAL_LOGGING_STACK_SIZE 1580 #endif @@ -188,7 +193,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types) { Backend_Type::FILESYSTEM, AP_Logger_File::probe }, #endif #if HAL_LOGGING_DATAFLASH_ENABLED - { Backend_Type::BLOCK, AP_Logger_DataFlash::probe }, + { Backend_Type::BLOCK, HAL_LOGGING_DATAFLASH_DRIVER::probe }, #endif #if HAL_LOGGING_MAVLINK_ENABLED { Backend_Type::MAVLINK, AP_Logger_MAVLink::probe }, @@ -206,7 +211,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types) LoggerMessageWriter_DFLogStart *message_writer = new LoggerMessageWriter_DFLogStart(); if (message_writer == nullptr) { - AP_BoardConfig::allocation_error("mesage writer"); + AP_BoardConfig::allocation_error("message writer"); } backends[_next_backend] = backend_config.probe_fn(*this, message_writer); if (backends[_next_backend] == nullptr) { diff --git a/libraries/AP_Logger/AP_Logger_Block.cpp b/libraries/AP_Logger/AP_Logger_Block.cpp index 4ead7fa554..9b6333ea21 100644 --- a/libraries/AP_Logger/AP_Logger_Block.cpp +++ b/libraries/AP_Logger/AP_Logger_Block.cpp @@ -21,17 +21,21 @@ AP_Logger_Block::AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStar AP_Logger_Backend(front, writer), writebuf(0) { - // buffer is used for both reads and writes so access must always be within the semaphore - buffer = (uint8_t *)hal.util->malloc_type(page_size_max, AP_HAL::Util::MEM_DMA_SAFE); - if (buffer == nullptr) { - AP_HAL::panic("Out of DMA memory for logging"); - } df_stats_clear(); } -// init is called after backend init +// Init() is called after driver Init(), it is the responsibility of the driver to make sure the +// device is ready to accept commands before Init() is called void AP_Logger_Block::Init(void) { + // buffer is used for both reads and writes so access must always be within the semaphore + buffer = (uint8_t *)hal.util->malloc_type(df_PageSize, AP_HAL::Util::MEM_DMA_SAFE); + if (buffer == nullptr) { + AP_HAL::panic("Out of DMA memory for logging"); + } + + //flash_test(); + if (CardInserted()) { // reserve space for version in last sector df_NumPages -= df_PagePerBlock; @@ -105,7 +109,7 @@ void AP_Logger_Block::FinishWrite(void) chip_full = true; return; } - SectorErase(df_PageAdr / df_PagePerBlock); + SectorErase(get_block(df_PageAdr)); } } @@ -864,9 +868,10 @@ void AP_Logger_Block::io_timer(void) WITH_SEMAPHORE(sem); const uint32_t sectors = df_NumPages / df_PagePerSector; - const uint32_t sectors_in_64k = 0x10000 / (df_PagePerSector * df_PageSize); + const uint32_t block_size = df_PagePerBlock * df_PageSize; + const uint32_t sectors_in_block = block_size / (df_PagePerSector * df_PageSize); uint32_t next_sector = get_sector(df_EraseFrom); - const uint32_t aligned_sector = sectors - (((df_NumPages - df_EraseFrom + 1) / df_PagePerSector) / sectors_in_64k) * sectors_in_64k; + const uint32_t aligned_sector = sectors - (((df_NumPages - df_EraseFrom + 1) / df_PagePerSector) / sectors_in_block) * sectors_in_block; while (next_sector < aligned_sector) { Sector4kErase(next_sector); io_timer_heartbeat = AP_HAL::millis(); @@ -875,9 +880,9 @@ void AP_Logger_Block::io_timer(void) uint16_t blocks_erased = 0; while (next_sector < sectors) { blocks_erased++; - SectorErase(next_sector / sectors_in_64k); + SectorErase(next_sector / sectors_in_block); io_timer_heartbeat = AP_HAL::millis(); - next_sector += sectors_in_64k; + next_sector += sectors_in_block; } status_msg = StatusMessage::RECOVERY_COMPLETE; df_EraseFrom = 0; @@ -928,4 +933,44 @@ void AP_Logger_Block::write_log_page() df_Write_FilePage++; } +void AP_Logger_Block::flash_test() +{ + const uint32_t pages_to_check = 128; + for (uint32_t i=1; i<=pages_to_check; i++) { + if ((i-1) % df_PagePerBlock == 0) { + printf("Block erase %u\n", get_block(i)); + SectorErase(get_block(i)); + } + memset(buffer, uint8_t(i), df_PageSize); + if (i<5) { + printf("Flash fill 0x%x\n", uint8_t(i)); + } else if (i==5) { + printf("Flash fill pages 5-%u\n", pages_to_check); + } + BufferToPage(i); + } + for (uint32_t i=1; i<=pages_to_check; i++) { + if (i<5) { + printf("Flash check 0x%x\n", uint8_t(i)); + } else if (i==5) { + printf("Flash check pages 5-%u\n", pages_to_check); + } + PageToBuffer(i); + uint32_t bad_bytes = 0; + uint32_t first_bad_byte = 0; + for (uint32_t j=0; j 0) { + printf("Test failed: page %u, %u of %u bad bytes, first=0x%x\n", + i, bad_bytes, df_PageSize, buffer[first_bad_byte]); + } + } +} + #endif // HAL_LOGGING_BLOCK_ENABLED diff --git a/libraries/AP_Logger/AP_Logger_Block.h b/libraries/AP_Logger/AP_Logger_Block.h index 0eb723629c..24a58ad12e 100644 --- a/libraries/AP_Logger/AP_Logger_Block.h +++ b/libraries/AP_Logger/AP_Logger_Block.h @@ -60,7 +60,6 @@ protected: uint32_t df_NumPages; volatile bool log_write_started; - static const uint16_t page_size_max = 256; uint8_t *buffer; uint32_t last_messagewrite_message_sent; @@ -74,6 +73,7 @@ private: virtual void Sector4kErase(uint32_t SectorAdr) = 0; virtual void StartErase() = 0; virtual bool InErase() = 0; + void flash_test(void); struct PACKED PageHeader { uint32_t FilePage; diff --git a/libraries/AP_Logger/AP_Logger_DataFlash.cpp b/libraries/AP_Logger/AP_Logger_DataFlash.cpp index 65e12f8739..ac85c280e3 100644 --- a/libraries/AP_Logger/AP_Logger_DataFlash.cpp +++ b/libraries/AP_Logger/AP_Logger_DataFlash.cpp @@ -109,9 +109,10 @@ bool AP_Logger_DataFlash::getSectorCount(void) // Read manufacturer ID uint8_t cmd = JEDEC_DEVICE_ID; - dev->transfer(&cmd, 1, buffer, 4); + uint8_t buf[4]; // buffer not yet allocated + dev->transfer(&cmd, 1, buf, 4); - uint32_t id = buffer[0] << 16 | buffer[1] << 8 | buffer[2]; + uint32_t id = buf[0] << 16 | buf[1] << 8 | buf[2]; uint32_t blocks = 0; @@ -302,29 +303,4 @@ void AP_Logger_DataFlash::WriteEnable(void) dev->transfer(&b, 1, nullptr, 0); } -void AP_Logger_DataFlash::flash_test() -{ - // wait for the chip to be ready, this has been moved from Init() - hal.scheduler->delay(2000); - - for (uint8_t i=1; i<=20; i++) { - printf("Flash fill %u\n", i); - if (i % df_PagePerBlock == 0) { - SectorErase(i / df_PagePerBlock); - } - memset(buffer, i, df_PageSize); - BufferToPage(i); - } - for (uint8_t i=1; i<=20; i++) { - printf("Flash check %u\n", i); - PageToBuffer(i); - for (uint32_t j=0; j dev; AP_HAL::Semaphore *dev_sem; diff --git a/libraries/AP_Logger/AP_Logger_W25N01GV.cpp b/libraries/AP_Logger/AP_Logger_W25N01GV.cpp new file mode 100644 index 0000000000..f4435835bc --- /dev/null +++ b/libraries/AP_Logger/AP_Logger_W25N01GV.cpp @@ -0,0 +1,319 @@ +/* + logging to a DataFlash block based storage device on SPI +*/ + + +#include + +#include "AP_Logger_W25N01GV.h" + +#if HAL_LOGGING_DATAFLASH_ENABLED + +#include + +extern const AP_HAL::HAL& hal; + +#define JEDEC_WRITE_ENABLE 0x06 +#define JEDEC_WRITE_DISABLE 0x04 +#define JEDEC_READ_STATUS 0x05 +#define JEDEC_WRITE_STATUS 0x01 +#define JEDEC_READ_DATA 0x03 +#define JEDEC_PAGE_DATA_READ 0x13 +#define JEDEC_FAST_READ 0x0b +#define JEDEC_DEVICE_ID 0x9F +#define JEDEC_PAGE_WRITE 0x02 +#define JEDEC_PROGRAM_EXECUTE 0x10 + +#define JEDEC_DEVICE_RESET 0xFF +#define JEDEC_BLOCK_ERASE 0xD8 // 128K erase + +#define JEDEC_STATUS_BUSY 0x01 +#define JEDEC_STATUS_WRITEPROTECT 0x02 + +#define W25N01G_STATUS_REG 0xC0 +#define W25N01G_PROT_REG 0xA0 +#define W25N01G_CONF_REG 0xB0 +#define W25N01G_STATUS_EFAIL 0x04 +#define W25N01G_STATUS_PFAIL 0x08 + +#define W25N01G_PROT_SRP1_ENABLE (1 << 0) +#define W25N01G_PROT_WP_E_ENABLE (1 << 1) +#define W25N01G_PROT_TB_ENABLE (1 << 2) +#define W25N01G_PROT_PB0_ENABLE (1 << 3) +#define W25N01G_PROT_PB1_ENABLE (1 << 4) +#define W25N01G_PROT_PB2_ENABLE (1 << 5) +#define W25N01G_PROT_PB3_ENABLE (1 << 6) +#define W25N01G_PROT_SRP2_ENABLE (1 << 7) + +#define W25N01G_CONFIG_ECC_ENABLE (1 << 4) +#define W25N01G_CONFIG_BUFFER_READ_MODE (1 << 3) + +#define W25N01G_TIMEOUT_PAGE_READ_US 60 // tREmax = 60us (ECC enabled) +#define W25N01G_TIMEOUT_PAGE_PROGRAM_US 700 // tPPmax = 700us +#define W25N01G_TIMEOUT_BLOCK_ERASE_MS 10 // tBEmax = 10ms +#define W25N01G_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms +#define W25N01G_NUM_BLOCKS 1024 + +#define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 + +void AP_Logger_W25N01GV::Init() +{ + dev = hal.spi->get_device("dataflash"); + if (!dev) { + AP_HAL::panic("PANIC: AP_Logger W25N01GV device not found"); + return; + } + + dev_sem = dev->get_semaphore(); + + if (!getSectorCount()) { + flash_died = true; + return; + } + + flash_died = false; + + // reset the device + WaitReady(); + { + WITH_SEMAPHORE(dev_sem); + uint8_t b = JEDEC_DEVICE_RESET; + dev->transfer(&b, 1, nullptr, 0); + } + hal.scheduler->delay(W25N01G_TIMEOUT_RESET_MS); + + // disable write protection + WriteStatusReg(W25N01G_PROT_REG, 0); + // enable ECC and buffer mode + WriteStatusReg(W25N01G_CONF_REG, W25N01G_CONFIG_ECC_ENABLE|W25N01G_CONFIG_BUFFER_READ_MODE); + + printf("W25N01GV status: SR-1=0x%x, SR-2=0x%x, SR-3=0x%x\n", + ReadStatusRegBits(W25N01G_PROT_REG), + ReadStatusRegBits(W25N01G_CONF_REG), + ReadStatusRegBits(W25N01G_STATUS_REG)); + + AP_Logger_Block::Init(); +} + +/* + wait for busy flag to be cleared + */ +void AP_Logger_W25N01GV::WaitReady() +{ + if (flash_died) { + return; + } + + uint32_t t = AP_HAL::millis(); + while (Busy()) { + hal.scheduler->delay_microseconds(100); + if (AP_HAL::millis() - t > 5000) { + printf("DataFlash: flash_died\n"); + flash_died = true; + break; + } + } +} + +bool AP_Logger_W25N01GV::getSectorCount(void) +{ + WaitReady(); + + WITH_SEMAPHORE(dev_sem); + + // Read manufacturer ID + uint8_t cmd = JEDEC_DEVICE_ID; + uint8_t buf[4]; // buffer not yet allocated + dev->transfer(&cmd, 1, buf, 4); + + uint32_t id = buf[1] << 16 | buf[2] << 8 | buf[3]; + + switch (id) { + case JEDEC_ID_WINBOND_W25N01GV: + df_PageSize = 2048; + df_PagePerBlock = 64; + df_PagePerSector = 64; // make sectors equivalent to block + break; + + default: + hal.scheduler->delay(2000); + printf("Unknown SPI Flash 0x%08x\n", id); + return false; + } + + df_NumPages = W25N01G_NUM_BLOCKS * df_PagePerBlock; + + printf("SPI Flash 0x%08x found pages=%u\n", id, df_NumPages); + return true; +} + +// Read the status register bits +uint8_t AP_Logger_W25N01GV::ReadStatusRegBits(uint8_t bits) +{ + WITH_SEMAPHORE(dev_sem); + uint8_t cmd[2] { JEDEC_READ_STATUS, bits }; + uint8_t status; + dev->transfer(cmd, 2, &status, 1); + return status; +} + +void AP_Logger_W25N01GV::WriteStatusReg(uint8_t reg, uint8_t bits) +{ + WaitReady(); + WITH_SEMAPHORE(dev_sem); + uint8_t cmd[3] = {JEDEC_WRITE_STATUS, reg, bits}; + dev->transfer(cmd, 3, nullptr, 0); +} + +bool AP_Logger_W25N01GV::Busy() +{ + uint8_t status = ReadStatusRegBits(W25N01G_STATUS_REG); + + if ((status & W25N01G_STATUS_PFAIL) != 0) { + printf("Program failure!\n"); + } + if ((status & W25N01G_STATUS_EFAIL) != 0) { + printf("Erase failure!\n"); + } + + return (status & JEDEC_STATUS_BUSY) != 0; +} + +/* + send a command with an address +*/ +void AP_Logger_W25N01GV::send_command_addr(uint8_t command, uint32_t PageAdr) +{ + uint8_t cmd[4]; + cmd[0] = command; + cmd[1] = 0; // dummy + cmd[2] = (PageAdr >> 8) & 0xff; + cmd[3] = (PageAdr >> 0) & 0xff; + + dev->transfer(cmd, 4, nullptr, 0); +} + +void AP_Logger_W25N01GV::PageToBuffer(uint32_t pageNum) +{ + if (pageNum == 0 || pageNum > df_NumPages+1) { + printf("Invalid page read %u\n", pageNum); + memset(buffer, 0xFF, df_PageSize); + return; + } + + WaitReady(); + + uint32_t PageAdr = (pageNum-1); + + { + WITH_SEMAPHORE(dev_sem); + // read page into internal buffer + send_command_addr(JEDEC_PAGE_DATA_READ, PageAdr); + } + + // read from internal buffer into our buffer + WaitReady(); + { + WITH_SEMAPHORE(dev_sem); + dev->set_chip_select(true); + uint8_t cmd[4]; + cmd[0] = JEDEC_READ_DATA; + cmd[1] = (0 >> 8) & 0xff; // column address zero + cmd[2] = (0 >> 0) & 0xff; // column address zero + cmd[3] = 0; // dummy + dev->transfer(cmd, 4, nullptr, 0); + dev->transfer(nullptr, 0, buffer, df_PageSize); + dev->set_chip_select(false); + } +} + +void AP_Logger_W25N01GV::BufferToPage(uint32_t pageNum) +{ + if (pageNum == 0 || pageNum > df_NumPages+1) { + printf("Invalid page write %u\n", pageNum); + return; + } + + WriteEnable(); + + uint32_t PageAdr = (pageNum-1); + { + WITH_SEMAPHORE(dev_sem); + + // write our buffer into internal buffer + dev->set_chip_select(true); + + uint8_t cmd[3]; + cmd[0] = JEDEC_PAGE_WRITE; + cmd[1] = (0 >> 8) & 0xff; // column address zero + cmd[2] = (0 >> 0) & 0xff; // column address zero + + dev->transfer(cmd, 3, nullptr, 0); + dev->transfer(buffer, df_PageSize, nullptr, 0); + dev->set_chip_select(false); + } + + // write from internal buffer into page + { + WITH_SEMAPHORE(dev_sem); + send_command_addr(JEDEC_PROGRAM_EXECUTE, PageAdr); + } +} + +/* + erase one sector (sizes varies with hw) +*/ +void AP_Logger_W25N01GV::SectorErase(uint32_t blockNum) +{ + WriteEnable(); + WITH_SEMAPHORE(dev_sem); + + uint32_t PageAdr = blockNum * df_PagePerBlock; + send_command_addr(JEDEC_BLOCK_ERASE, PageAdr); +} + +/* + erase one 4k sector +*/ +void AP_Logger_W25N01GV::Sector4kErase(uint32_t sectorNum) +{ + SectorErase(sectorNum); +} + +void AP_Logger_W25N01GV::StartErase() +{ + WriteEnable(); + + WITH_SEMAPHORE(dev_sem); + + // just erase the first block, others will follow in InErase + send_command_addr(JEDEC_BLOCK_ERASE, 0); + + erase_block = 1; + erase_start_ms = AP_HAL::millis(); + printf("Dataflash: erase started\n"); +} + +bool AP_Logger_W25N01GV::InErase() +{ + if (erase_start_ms && !Busy()) { + if (erase_block < W25N01G_NUM_BLOCKS) { + SectorErase(erase_block++); + } else { + printf("Dataflash: erase done (%u ms)\n", AP_HAL::millis() - erase_start_ms); + erase_start_ms = 0; + erase_block = 0; + } + } + return erase_start_ms != 0; +} + +void AP_Logger_W25N01GV::WriteEnable(void) +{ + WaitReady(); + WITH_SEMAPHORE(dev_sem); + uint8_t b = JEDEC_WRITE_ENABLE; + dev->transfer(&b, 1, nullptr, 0); +} + +#endif // HAL_LOGGING_DATAFLASH_ENABLED diff --git a/libraries/AP_Logger/AP_Logger_W25N01GV.h b/libraries/AP_Logger/AP_Logger_W25N01GV.h new file mode 100644 index 0000000000..0865d355ed --- /dev/null +++ b/libraries/AP_Logger/AP_Logger_W25N01GV.h @@ -0,0 +1,47 @@ +/* + logging for block based dataflash devices on SPI + */ +#pragma once + +#include + +#include "AP_Logger_Block.h" + +#if HAL_LOGGING_DATAFLASH_ENABLED + +class AP_Logger_W25N01GV : public AP_Logger_Block { +public: + AP_Logger_W25N01GV(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) : + AP_Logger_Block(front, writer) {} + static AP_Logger_Backend *probe(AP_Logger &front, + LoggerMessageWriter_DFLogStart *ls) { + return new AP_Logger_W25N01GV(front, ls); + } + void Init(void) override; + bool CardInserted() const override { return !flash_died && df_NumPages > 0; } + +private: + void BufferToPage(uint32_t PageAdr) override; + void PageToBuffer(uint32_t PageAdr) override; + void SectorErase(uint32_t SectorAdr) override; + void Sector4kErase(uint32_t SectorAdr) override; + void StartErase() override; + bool InErase() override; + void send_command_addr(uint8_t cmd, uint32_t address); + void WaitReady(); + bool Busy(); + uint8_t ReadStatusRegBits(uint8_t bits); + void WriteStatusReg(uint8_t reg, uint8_t bits); + + void WriteEnable(); + bool getSectorCount(void); + + AP_HAL::OwnPtr dev; + AP_HAL::Semaphore *dev_sem; + + bool flash_died; + uint32_t erase_start_ms; + uint16_t erase_block; +}; + +#endif // HAL_LOGGING_DATAFLASH_ENABLED