diff --git a/cmake/at32-bootloader.cmake b/cmake/at32-bootloader.cmake index 857ae08a524..0caba562747 100644 --- a/cmake/at32-bootloader.cmake +++ b/cmake/at32-bootloader.cmake @@ -21,7 +21,7 @@ main_sources(BOOTLOADER_SOURCES drivers/time.c drivers/timer.c drivers/flash_m25p16.c - drivers/flash_w25n01g.c + drivers/flash_w25n.c drivers/flash.c fc/firmware_update_common.c diff --git a/docs/Blackbox.md b/docs/Blackbox.md index d0e9fe5dc61..3d2fb68290d 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -120,6 +120,8 @@ These chips are also supported: * Winbond W25Q64 - 64 Mbit / 8 MByte * Micron N25Q0128 - 128 Mbit / 16 MByte * Winbond W25Q128 - 128 Mbit / 16 MByte +* Winbond W25N01 - 1 Gbit / 128 MByte +* Winbond W25N02 - 2 Gbit / 256 MByte #### Enable recording to dataflash On the Configurator's CLI tab, you must enter `set blackbox_device=SPIFLASH` to switch to logging to an onboard dataflash chip, then save. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 3216bbdb8ee..41a7668c77f 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -179,8 +179,8 @@ main_sources(COMMON_SRC drivers/flash.h drivers/flash_m25p16.c drivers/flash_m25p16.h - drivers/flash_w25n01g.c - drivers/flash_w25n01g.h + drivers/flash_w25n.c + drivers/flash_w25n.h drivers/gimbal_common.h drivers/gimbal_common.c drivers/headtracker_common.h diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 65a21f8fce1..5a3e6dba453 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -142,7 +142,7 @@ typedef enum { DEVHW_MS4525, // Pitot meter DEVHW_DLVR, // Pitot meter DEVHW_M25P16, // SPI NOR flash - DEVHW_W25N01G, // SPI 128MB flash + DEVHW_W25N, // SPI 128MB or 256MB flash from Winbond W25N family DEVHW_UG2864, // I2C OLED display DEVHW_SDCARD, // Generic SD-Card DEVHW_IRLOCK, // IR-Lock visual positioning hardware diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index 145aeafd56b..392c7680519 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -30,7 +30,7 @@ #include "flash.h" #include "flash_m25p16.h" -#include "flash_w25n01g.h" +#include "flash_w25n.h" #include "common/time.h" @@ -56,17 +56,17 @@ static flashDriver_t flashDrivers[] = { }, #endif -#ifdef USE_FLASH_W25N01G +#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) { - .init = w25n01g_init, - .isReady = w25n01g_isReady, - .waitForReady = w25n01g_waitForReady, - .eraseSector = w25n01g_eraseSector, - .eraseCompletely = w25n01g_eraseCompletely, - .pageProgram = w25n01g_pageProgram, - .readBytes = w25n01g_readBytes, - .getGeometry = w25n01g_getGeometry, - .flush = w25n01g_flush + .init = w25n_init, + .isReady = w25n_isReady, + .waitForReady = w25n_waitForReady, + .eraseSector = w25n_eraseSector, + .eraseCompletely = w25n_eraseCompletely, + .pageProgram = w25n_pageProgram, + .readBytes = w25n_readBytes, + .getGeometry = w25n_getGeometry, + .flush = w25n_flush }, #endif diff --git a/src/main/drivers/flash_w25n.c b/src/main/drivers/flash_w25n.c new file mode 100644 index 00000000000..2791d3d89e1 --- /dev/null +++ b/src/main/drivers/flash_w25n.c @@ -0,0 +1,555 @@ +/* + * This file is part of INAV. + * + * INAV are free software. You can redistribute + * this software and/or modify this software 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. + * + * INAV are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/time.h" + +#include "flash_w25n.h" + +// Device size parameters +#define W25N_PAGE_SIZE 2048 +#define W25N_PAGES_PER_BLOCK 64 + +#define W25N01GV_BLOCKS_PER_DIE 1024 +#define W25N02KV_BLOCKS_PER_DIE 2048 + + + +// BB replacement area +#define W25N_BB_MARKER_BLOCKS 1 +#define W25N_BB_REPLACEMENT_BLOCKS 20 +#define W25N_BB_MANAGEMENT_BLOCKS (W25N_BB_REPLACEMENT_BLOCKS + W25N_BB_MARKER_BLOCKS) + +// blocks are zero-based index +#define W25N_BB_REPLACEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_REPLACEMENT_BLOCKS) +#define W25N_BB_MANAGEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_MANAGEMENT_BLOCKS) +#define W25N_BB_MARKER_BLOCK (W25N_BB_REPLACEMENT_START_BLOCK - W25N_BB_MARKER_BLOCKS) + +// Instructions +#define W25N_INSTRUCTION_RDID 0x9F +#define W25N_INSTRUCTION_DEVICE_RESET 0xFF +#define W25N_INSTRUCTION_READ_STATUS_REG 0x05 +#define W25N_INSTRUCTION_READ_STATUS_ALTERNATE_REG 0x0F +#define W25N_INSTRUCTION_WRITE_STATUS_REG 0x01 +#define W25N_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F +#define W25N_INSTRUCTION_WRITE_ENABLE 0x06 +#define W25N_INSTRUCTION_DIE_SELECT 0xC2 +#define W25N_INSTRUCTION_BLOCK_ERASE 0xD8 +#define W25N_INSTRUCTION_READ_BBM_LUT 0xA5 +#define W25N_INSTRUCTION_BB_MANAGEMENT 0xA1 +#define W25N_INSTRUCTION_PROGRAM_DATA_LOAD 0x02 +#define W25N_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD 0x84 +#define W25N_INSTRUCTION_PROGRAM_EXECUTE 0x10 +#define W25N_INSTRUCTION_PAGE_DATA_READ 0x13 +#define W25N_INSTRUCTION_READ_DATA 0x03 +#define W25N_INSTRUCTION_FAST_READ 0x1B +#define W25N_INSTRUCTION_FAST_READ_QUAD_OUTPUT 0x6B + +// Config/status register addresses +#define W25N_PROT_REG 0xA0 +#define W25N_CONF_REG 0xB0 +#define W25N_STAT_REG 0xC0 + +// Bits in config/status register 1 (W25N_PROT_REG) +#define W25N_PROT_CLEAR (0) +#define W25N_PROT_SRP1_ENABLE (1 << 0) +#define W25N_PROT_WP_E_ENABLE (1 << 1) +#define W25N_PROT_TB_ENABLE (1 << 2) +#define W25N_PROT_PB0_ENABLE (1 << 3) +#define W25N_PROT_PB1_ENABLE (1 << 4) +#define W25N_PROT_PB2_ENABLE (1 << 5) +#define W25N_PROT_PB3_ENABLE (1 << 6) +#define W25N_PROT_SRP2_ENABLE (1 << 7) + +// Bits in config/status register 2 (W25N_CONF_REG) +#define W25N_CONFIG_ECC_ENABLE (1 << 4) +#define W25N_CONFIG_BUFFER_READ_MODE (1 << 3) + +// Bits in config/status register 3 (W25N_STATREG) +#define W25N_STATUS_BBM_LUT_FULL (1 << 6) +#define W25N_STATUS_FLAG_ECC_POS 4 +#define W25N_STATUS_FLAG_ECC_MASK ((1 << 5)|(1 << 4)) +#define W25N_STATUS_FLAG_ECC(status) (((status) & W25N_STATUS_FLAG_ECC_MASK) >> 4) +#define W25N_STATUS_PROGRAM_FAIL (1 << 3) +#define W25N_STATUS_ERASE_FAIL (1 << 2) +#define W25N_STATUS_FLAG_WRITE_ENABLED (1 << 1) +#define W25N_STATUS_FLAG_BUSY (1 << 0) +#define W25N_BBLUT_TABLE_ENTRY_COUNT 20 +#define W25N_BBLUT_TABLE_ENTRY_SIZE 4 // in bytes + +// Bits in LBA for BB LUT +#define W25N_BBLUT_STATUS_ENABLED (1 << 15) +#define W25N_BBLUT_STATUS_INVALID (1 << 14) +#define W25N_BBLUT_STATUS_MASK (W25N_BBLUT_STATUS_ENABLED | W25N_BBLUT_STATUS_INVALID) + +// Some useful defs and macros +#define W25N_LINEAR_TO_COLUMN(laddr) ((laddr) % W25N_PAGE_SIZE) +#define W25N_LINEAR_TO_PAGE(laddr) ((laddr) / W25N_PAGE_SIZE) +#define W25N_LINEAR_TO_BLOCK(laddr) (W25N_LINEAR_TO_PAGE(laddr) / W25N_PAGES_PER_BLOCK) +#define W25N_BLOCK_TO_PAGE(block) ((block) * W25N_PAGES_PER_BLOCK) +#define W25N_BLOCK_TO_LINEAR(block) (W25N_BLOCK_TO_PAGE(block) * W25N_PAGE_SIZE) + +// IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver +// The timeout values (2ms minimum to avoid 1 tick advance in consecutive calls to millis). +#define W25N_TIMEOUT_PAGE_READ_MS 2 // tREmax = 60us (ECC enabled) +#define W25N_TIMEOUT_PAGE_PROGRAM_MS 2 // tPPmax = 700us +#define W25N_TIMEOUT_BLOCK_ERASE_MS 15 // tBEmax = 10ms +#define W25N_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms + +// Sizes (in bits) +#define W28N_STATUS_REGISTER_SIZE 8 +#define W28N_STATUS_PAGE_ADDRESS_SIZE 16 +#define W28N_STATUS_COLUMN_ADDRESS_SIZE 16 + +// JEDEC ID +#define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 +#define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22 + +static busDevice_t *busDev = NULL; +static flashGeometry_t geometry; + +/* + * Whether we've performed an action that could have made the device busy for writes. + * + * This allows us to avoid polling for writable status when it is definitely ready already. + */ +static bool couldBeBusy = false; + +static timeMs_t timeoutAt = 0; + +static bool w25n_waitForReadyInternal(void); + +static void w25n_setTimeout(timeMs_t timeoutMillis) +{ + timeMs_t now = millis(); + timeoutAt = now + timeoutMillis; + couldBeBusy = true; +} + +/** + * Send the given command byte to the device. + */ +static void w25n_performOneByteCommand(uint8_t command) +{ + busTransfer(busDev, NULL, &command, 1); +} + +static void w25n_performCommandWithPageAddress(uint8_t command, uint32_t pageAddress) +{ + uint8_t cmd[4] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff}; + busTransfer(busDev, NULL, cmd, sizeof(cmd)); +} + +static uint8_t w25n_readRegister(uint8_t reg) +{ + uint8_t command[3] = { W25N_INSTRUCTION_READ_STATUS_REG, reg, 0 }; + uint8_t in[3]; + + busTransfer(busDev, in, command, sizeof(command)); + + return in[2]; +} + +static void w25n_writeRegister(uint8_t reg, uint8_t data) +{ + uint8_t cmd[3] = { W25N_INSTRUCTION_WRITE_STATUS_REG, reg, data }; + busTransfer(busDev, NULL, cmd, sizeof(cmd)); +} + +static void w25n_deviceReset(void) +{ + w25n_performOneByteCommand(W25N_INSTRUCTION_DEVICE_RESET); + w25n_setTimeout(W25N_TIMEOUT_RESET_MS); + w25n_waitForReadyInternal(); + // Protection for upper 1/32 (BP[3:0] = 0101, TB=0), WP-E on; to protect bad block replacement area + // DON'T DO THIS. This will prevent writes through the bblut as well. + // w25n_writeRegister(busdev, W25N_PROT_REG, W25N_PROT_PB0_ENABLE|W25N_PROT_PB2_ENABLE|W25N_PROT_WP_E_ENABLE); + // No protection, WP-E off, WP-E prevents use of IO2 + w25n_writeRegister(W25N_PROT_REG, W25N_PROT_CLEAR); + // Buffered read mode (BUF = 1), ECC enabled (ECC = 1) + w25n_writeRegister(W25N_CONF_REG, W25N_CONFIG_ECC_ENABLE | W25N_CONFIG_BUFFER_READ_MODE); +} + +bool w25n_isReady(void) +{ + uint8_t status = w25n_readRegister(W25N_STAT_REG); + + // If couldBeBusy is false, don't bother to poll the flash chip for its status + couldBeBusy = couldBeBusy && ((status & W25N_STATUS_FLAG_BUSY) != 0); + + return !couldBeBusy; +} + +static bool w25n_waitForReadyInternal(void) +{ + while (!w25n_isReady()) { + timeMs_t now = millis(); + if (cmp32(now, timeoutAt) >= 0) { + return false; + } + } + timeoutAt = 0; + return true; +} + +bool w25n_waitForReady(timeMs_t timeoutMillis) +{ + w25n_setTimeout(timeoutMillis); + return w25n_waitForReadyInternal(); +} + +/** + * The flash requires this write enable command to be sent before commands that would cause + * a write like program and erase. + */ +static void w25n_writeEnable(void) +{ + w25n_performOneByteCommand(W25N_INSTRUCTION_WRITE_ENABLE); + + // Assume that we're about to do some writing, so the device is just about to become busy + couldBeBusy = true; +} + +bool w25n_detect(uint32_t chipID) +{ + switch (chipID) { + case JEDEC_ID_WINBOND_W25N01GV: + geometry.sectors = W25N01GV_BLOCKS_PER_DIE; // Blocks + geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks + geometry.pageSize = W25N_PAGE_SIZE; + break; + case JEDEC_ID_WINBOND_W25N02KV: + geometry.sectors = W25N02KV_BLOCKS_PER_DIE; // Blocks + geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks + geometry.pageSize = W25N_PAGE_SIZE; + break; + + default: + // Unsupported chip + geometry.sectors = 0; + geometry.pagesPerSector = 0; + geometry.sectorSize = 0; + geometry.totalSize = 0; + return false; + } + + geometry.flashType = FLASH_TYPE_NAND; + geometry.sectorSize = geometry.pagesPerSector * geometry.pageSize; + geometry.totalSize = geometry.sectorSize * geometry.sectors; + + /*flashPartitionSet(FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT, + W25N_BB_MANAGEMENT_START_BLOCK, + W25N_BB_MANAGEMENT_START_BLOCK + W25N_BB_MANAGEMENT_BLOCKS - 1);*/ + + couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be + + w25n_deviceReset(); + + // Upper 4MB (32 blocks * 128KB/block) will be used for bad block replacement area. + // Blocks in this area are only written through bad block LUT, + // and factory written bad block marker in unused blocks are retained. + // When a replacement block is required, + // (1) "Read BB LUT" command is used to obtain the last block mapped, + // (2) blocks after the last block is scanned for a good block, + // (3) the first good block is used for replacement, and the BB LUT is updated. + // There are only 20 BB LUT entries, and there are 32 replacement blocks. + // There will be a least chance of running out of replacement blocks. + // If it ever run out, the device becomes unusable. + + return true; +} + +/** + * Erase a sector full of bytes to all 1's at the given byte offset in the flash chip. + */ +void w25n_eraseSector(uint32_t address) +{ + w25n_waitForReadyInternal(); + w25n_writeEnable(); + w25n_performCommandWithPageAddress(W25N_INSTRUCTION_BLOCK_ERASE, W25N_LINEAR_TO_PAGE(address)); + w25n_setTimeout(W25N_TIMEOUT_BLOCK_ERASE_MS); +} + +// W25N does not support full chip erase. +// Call eraseSector repeatedly. +void w25n_eraseCompletely(void) +{ + for (uint32_t block = 0; block < geometry.sectors; block++) { + w25n_eraseSector(W25N_BLOCK_TO_LINEAR(block)); + } +} + +static void w25n_programDataLoad(uint16_t columnAddress, const uint8_t *data, int length) +{ + w25n_waitForReadyInternal(); + + uint8_t cmd[3] = {W25N_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; + + busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; + busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); + + w25n_setTimeout(W25N_TIMEOUT_PAGE_PROGRAM_MS); +} + +static void w25n_randomProgramDataLoad(uint16_t columnAddress, const uint8_t *data, int length) +{ + w25n_waitForReadyInternal(); + + uint8_t cmd[3] = {W25N_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; + + busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; + busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); + + w25n_setTimeout(W25N_TIMEOUT_PAGE_PROGRAM_MS); +} + +static void w25n_programExecute(uint32_t pageAddress) +{ + w25n_waitForReadyInternal(); + w25n_performCommandWithPageAddress(W25N_INSTRUCTION_PROGRAM_EXECUTE, pageAddress); + w25n_setTimeout(W25N_TIMEOUT_PAGE_PROGRAM_MS); +} + +// Writes are done in three steps: +// (1) Load internal data buffer with data to write +// - We use "Random Load Program Data", as "Load Program Data" resets unused data bytes in the buffer to 0xff. +// - Each "Random Load Program Data" instruction must be accompanied by at least a single data. +// - Each "Random Load Program Data" instruction terminates at the rising of CS. +// (2) Enable write +// (3) Issue "Execute Program" +// +/* +flashfs page program behavior +- Single program never crosses page boundary. +- Except for this characteristic, it program arbitral size. +- Write address is, naturally, not a page boundary. +To cope with this behavior. +pageProgramBegin: +If buffer is dirty and programLoadAddress != address, then the last page is a partial write; +issue PAGE_PROGRAM_EXECUTE to flash buffer contents, clear dirty and record the address as programLoadAddress and programStartAddress. +Else do nothing. +pageProgramContinue: +Mark buffer as dirty. +If programLoadAddress is on page boundary, then issue PROGRAM_LOAD_DATA, else issue RANDOM_PROGRAM_LOAD_DATA. +Update programLoadAddress. +Optionally observe the programLoadAddress, and if it's on page boundary, issue PAGE_PROGRAM_EXECUTE. +pageProgramFinish: +Observe programLoadAddress. If it's on page boundary, issue PAGE_PROGRAM_EXECUTE and clear dirty, else just return. +If pageProgramContinue observes the page boundary, then do nothing(?). +*/ +bool bufferDirty = false; +bool isProgramming = false; +static uint32_t programStartAddress; +static uint32_t programLoadAddress; +static uint32_t currentPage = UINT32_MAX; + +void w25n_pageProgramBegin(uint32_t address) +{ + if (bufferDirty) { + if (address != programLoadAddress) { + w25n_waitForReadyInternal(); + isProgramming = false; + w25n_writeEnable(); + w25n_programExecute(W25N_LINEAR_TO_PAGE(programStartAddress)); + bufferDirty = false; + isProgramming = true; + } + } else { + programStartAddress = programLoadAddress = address; + } +} + +void w25n_pageProgramContinue(const uint8_t *data, int length) +{ + // Check for page boundary overrun + w25n_waitForReadyInternal(); + w25n_writeEnable(); + isProgramming = false; + if (!bufferDirty) { + w25n_programDataLoad(W25N_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); + } else { + w25n_randomProgramDataLoad(W25N_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); + } + // XXX Test if write enable is reset after each data loading. + bufferDirty = true; + programLoadAddress += length; +} + +void w25n_pageProgramFinish(void) +{ + if (bufferDirty && W25N_LINEAR_TO_COLUMN(programLoadAddress) == 0) { + currentPage = W25N_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written + w25n_programExecute(W25N_LINEAR_TO_PAGE(programStartAddress)); + bufferDirty = false; + isProgramming = true; + programStartAddress = programLoadAddress; + } +} + +/* + * Write bytes to a flash page. Address must not cross a page boundary. + * + * Bits can only be set to zero, not from zero back to one again. In order to set bits to 1, use the erase command. + * + * Length must be smaller than the page size. + * + * This will wait for the flash to become ready before writing begins. + * + * Datasheet indicates typical programming time is 0.8ms for 256 bytes, 0.2ms for 64 bytes, 0.05ms for 16 bytes. + * (Although the maximum possible write time is noted as 5ms). + * + * If you want to write multiple buffers (whose sum of sizes is still not more than the page size) then you can + * break this operation up into one beginProgram call, one or more continueProgram calls, and one finishProgram call. + */ +uint32_t w25n_pageProgram(uint32_t address, const uint8_t *data, int length) +{ + w25n_pageProgramBegin(address); + w25n_pageProgramContinue((uint8_t *)data, length); + w25n_pageProgramFinish(); + + return address + length; +} + +void w25n_flush(void) +{ + if (bufferDirty) { + currentPage = W25N_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written + w25n_programExecute(W25N_LINEAR_TO_PAGE(programStartAddress)); + bufferDirty = false; + isProgramming = true; + } else { + isProgramming = false; + } +} + +void w25n_addError(uint32_t address, uint8_t code) +{ + UNUSED(address); + UNUSED(code); +} + +/* + * Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie + * on a page boundary). + * + * Waits up to W25N_TIMEOUT_PAGE_READ_MS milliseconds for the flash to become ready before reading. + * + * The number of bytes actually read is returned, which can be zero if an error or timeout occurred. + */ +// Continuous read mode (BUF = 0): +// (1) "Page Data Read" command is executed for the page pointed by address +// (2) "Read Data" command is executed for bytes not requested and data are discarded +// (3) "Read Data" command is executed and data are stored directly into caller's buffer +// +// Buffered read mode (BUF = 1), non-read ahead +// (1) If currentBufferPage != requested page, then issue PAGE_DATA_READ on requested page. +// (2) Compute transferLength as smaller of remaining length and requested length. +// (3) Issue READ_DATA on column address. +// (4) Return transferLength. +int w25n_readBytes(uint32_t address, uint8_t *buffer, int length) +{ + uint32_t targetPage = W25N_LINEAR_TO_PAGE(address); + + if (currentPage != targetPage) { + if (!w25n_waitForReadyInternal()) { + return 0; + } + currentPage = UINT32_MAX; + w25n_performCommandWithPageAddress(W25N_INSTRUCTION_PAGE_DATA_READ, targetPage); + if (!w25n_waitForReady(W25N_TIMEOUT_PAGE_READ_MS)) { + return 0; + } + currentPage = targetPage; + } + + uint16_t column = W25N_LINEAR_TO_COLUMN(address); + uint16_t transferLength; + + if (length > W25N_PAGE_SIZE - column) { + transferLength = W25N_PAGE_SIZE - column; + } else { + transferLength = length; + } + + const uint8_t cmd[4] = {W25N_INSTRUCTION_READ_DATA, (column >> 8) & 0xff, (column >> 0) & 0xff, 0}; + + busTransferDescriptor_t readDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = transferLength, .rxBuf = buffer, .txBuf = NULL}}; + busTransferMultiple(busDev, readDescr, ARRAYLEN(readDescr)); + + if (!w25n_waitForReady(W25N_TIMEOUT_PAGE_READ_MS)) { + return 0; + } + + // Check ECC + uint8_t statReg = w25n_readRegister(W25N_STAT_REG); + uint8_t eccCode = W25N_STATUS_FLAG_ECC(statReg); + switch (eccCode) { + case 0: // Successful read, no ECC correction + break; + + case 1: // Successful read with ECC correction + case 2: // Uncorrectable ECC in a single page + case 3: // Uncorrectable ECC in multiple pages + w25n_addError(address, eccCode); + w25n_deviceReset(); + break; + } + + return transferLength; +} + +/** + * Fetch information about the detected flash chip layout. + * + * Can be called before calling w25n_init() (the result would have totalSize = 0). + */ +const flashGeometry_t* w25n_getGeometry(void) +{ + return &geometry; +} + +bool w25n_init(int flashNumToUse) +{ + busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_W25N, flashNumToUse, OWNER_FLASH); + if (busDev == NULL) { + return false; + } + + uint8_t in[4] = { 0 }; + uint32_t chipID; + + delay(50); // short delay required after initialisation of SPI device instance. + + busReadBuf(busDev, W25N_INSTRUCTION_RDID, in, sizeof(in)); + + chipID = (in[1] << 16) | (in[2] << 8) | (in[3]); + + return w25n_detect(chipID); +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/flash_w25n01g.h b/src/main/drivers/flash_w25n.h similarity index 65% rename from src/main/drivers/flash_w25n01g.h rename to src/main/drivers/flash_w25n.h index 032234005b6..516cefea8c5 100644 --- a/src/main/drivers/flash_w25n01g.h +++ b/src/main/drivers/flash_w25n.h @@ -24,18 +24,18 @@ #include "flash.h" #include "drivers/io_types.h" -bool w25n01g_init(int flashNumToUse); +bool w25n_init(int flashNumToUse); -void w25n01g_eraseSector(uint32_t address); -void w25n01g_eraseCompletely(void); +void w25n_eraseSector(uint32_t address); +void w25n_eraseCompletely(void); -uint32_t w25n01g_pageProgram(uint32_t address, const uint8_t *data, int length); +uint32_t w25n_pageProgram(uint32_t address, const uint8_t *data, int length); -void w25n01g_flush(void); +void w25n_flush(void); -int w25n01g_readBytes(uint32_t address, uint8_t *buffer, int length); +int w25n_readBytes(uint32_t address, uint8_t *buffer, int length); -bool w25n01g_isReady(void); -bool w25n01g_waitForReady(timeMs_t timeoutMillis); +bool w25n_isReady(void); +bool w25n_waitForReady(timeMs_t timeoutMillis); -const flashGeometry_t* w25n01g_getGeometry(void); \ No newline at end of file +const flashGeometry_t* w25n_getGeometry(void); \ No newline at end of file diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c deleted file mode 100644 index bb85fd90cb6..00000000000 --- a/src/main/drivers/flash_w25n01g.c +++ /dev/null @@ -1,575 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV are free software. You can redistribute - * this software and/or modify this software 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. - * - * INAV are distributed in the hope that they - * 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 software. - * - * If not, see . - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_FLASH_W25N01G - -#include "drivers/bus.h" -#include "drivers/io.h" -#include "drivers/time.h" - -#include "flash_w25n01g.h" - -// Device size parameters -#define W25N01G_PAGE_SIZE 2048 -#define W25N01G_PAGES_PER_BLOCK 64 -#define W25N01G_BLOCKS_PER_DIE 1024 - -// BB replacement area -#define W25N01G_BB_MARKER_BLOCKS 1 -#define W25N01G_BB_REPLACEMENT_BLOCKS 20 -#define W25N01G_BB_MANAGEMENT_BLOCKS (W25N01G_BB_REPLACEMENT_BLOCKS + W25N01G_BB_MARKER_BLOCKS) - -// blocks are zero-based index -#define W25N01G_BB_REPLACEMENT_START_BLOCK (W25N01G_BLOCKS_PER_DIE - W25N01G_BB_REPLACEMENT_BLOCKS) -#define W25N01G_BB_MANAGEMENT_START_BLOCK (W25N01G_BLOCKS_PER_DIE - W25N01G_BB_MANAGEMENT_BLOCKS) -#define W25N01G_BB_MARKER_BLOCK (W25N01G_BB_REPLACEMENT_START_BLOCK - W25N01G_BB_MARKER_BLOCKS) - -// Instructions -#define W25N01G_INSTRUCTION_RDID 0x9F -#define W25N01G_INSTRUCTION_DEVICE_RESET 0xFF -#define W25N01G_INSTRUCTION_READ_STATUS_REG 0x05 -#define W25N01G_INSTRUCTION_READ_STATUS_ALTERNATE_REG 0x0F -#define W25N01G_INSTRUCTION_WRITE_STATUS_REG 0x01 -#define W25N01G_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F -#define W25N01G_INSTRUCTION_WRITE_ENABLE 0x06 -#define W25N01G_INSTRUCTION_DIE_SELECT 0xC2 -#define W25N01G_INSTRUCTION_BLOCK_ERASE 0xD8 -#define W25N01G_INSTRUCTION_READ_BBM_LUT 0xA5 -#define W25N01G_INSTRUCTION_BB_MANAGEMENT 0xA1 -#define W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD 0x02 -#define W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD 0x84 -#define W25N01G_INSTRUCTION_PROGRAM_EXECUTE 0x10 -#define W25N01G_INSTRUCTION_PAGE_DATA_READ 0x13 -#define W25N01G_INSTRUCTION_READ_DATA 0x03 -#define W25N01G_INSTRUCTION_FAST_READ 0x1B -#define W25N01G_INSTRUCTION_FAST_READ_QUAD_OUTPUT 0x6B - -// Config/status register addresses -#define W25N01G_PROT_REG 0xA0 -#define W25N01G_CONF_REG 0xB0 -#define W25N01G_STAT_REG 0xC0 - -// Bits in config/status register 1 (W25N01G_PROT_REG) -#define W25N01G_PROT_CLEAR (0) -#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) - -// Bits in config/status register 2 (W25N01G_CONF_REG) -#define W25N01G_CONFIG_ECC_ENABLE (1 << 4) -#define W25N01G_CONFIG_BUFFER_READ_MODE (1 << 3) - -// Bits in config/status register 3 (W25N01G_STATREG) -#define W25N01G_STATUS_BBM_LUT_FULL (1 << 6) -#define W25N01G_STATUS_FLAG_ECC_POS 4 -#define W25N01G_STATUS_FLAG_ECC_MASK ((1 << 5)|(1 << 4)) -#define W25N01G_STATUS_FLAG_ECC(status) (((status) & W25N01G_STATUS_FLAG_ECC_MASK) >> 4) -#define W25N01G_STATUS_PROGRAM_FAIL (1 << 3) -#define W25N01G_STATUS_ERASE_FAIL (1 << 2) -#define W25N01G_STATUS_FLAG_WRITE_ENABLED (1 << 1) -#define W25N01G_STATUS_FLAG_BUSY (1 << 0) -#define W25N01G_BBLUT_TABLE_ENTRY_COUNT 20 -#define W25N01G_BBLUT_TABLE_ENTRY_SIZE 4 // in bytes - -// Bits in LBA for BB LUT -#define W25N01G_BBLUT_STATUS_ENABLED (1 << 15) -#define W25N01G_BBLUT_STATUS_INVALID (1 << 14) -#define W25N01G_BBLUT_STATUS_MASK (W25N01G_BBLUT_STATUS_ENABLED | W25N01G_BBLUT_STATUS_INVALID) - -// Some useful defs and macros -#define W25N01G_LINEAR_TO_COLUMN(laddr) ((laddr) % W25N01G_PAGE_SIZE) -#define W25N01G_LINEAR_TO_PAGE(laddr) ((laddr) / W25N01G_PAGE_SIZE) -#define W25N01G_LINEAR_TO_BLOCK(laddr) (W25N01G_LINEAR_TO_PAGE(laddr) / W25N01G_PAGES_PER_BLOCK) -#define W25N01G_BLOCK_TO_PAGE(block) ((block) * W25N01G_PAGES_PER_BLOCK) -#define W25N01G_BLOCK_TO_LINEAR(block) (W25N01G_BLOCK_TO_PAGE(block) * W25N01G_PAGE_SIZE) - -// IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver -// The timeout values (2ms minimum to avoid 1 tick advance in consecutive calls to millis). -#define W25N01G_TIMEOUT_PAGE_READ_MS 2 // tREmax = 60us (ECC enabled) -#define W25N01G_TIMEOUT_PAGE_PROGRAM_MS 2 // tPPmax = 700us -#define W25N01G_TIMEOUT_BLOCK_ERASE_MS 15 // tBEmax = 10ms -#define W25N01G_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms - -// Sizes (in bits) -#define W28N01G_STATUS_REGISTER_SIZE 8 -#define W28N01G_STATUS_PAGE_ADDRESS_SIZE 16 -#define W28N01G_STATUS_COLUMN_ADDRESS_SIZE 16 - -// JEDEC ID -#define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 -#define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22 - -static busDevice_t *busDev = NULL; -static flashGeometry_t geometry; - -/* - * Whether we've performed an action that could have made the device busy for writes. - * - * This allows us to avoid polling for writable status when it is definitely ready already. - */ -static bool couldBeBusy = false; - -static timeMs_t timeoutAt = 0; - -static bool w25n01g_waitForReadyInternal(void); - -static void w25n01g_setTimeout(timeMs_t timeoutMillis) -{ - timeMs_t now = millis(); - timeoutAt = now + timeoutMillis; - couldBeBusy = true; -} - -/** - * Send the given command byte to the device. - */ -static void w25n01g_performOneByteCommand(uint8_t command) -{ - busTransfer(busDev, NULL, &command, 1); -} - -static void w25n01g_performCommandWithPageAddress(uint8_t command, uint32_t pageAddress) -{ - uint8_t cmd[4] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff}; - busTransfer(busDev, NULL, cmd, sizeof(cmd)); -} - -static uint8_t w25n01g_readRegister(uint8_t reg) -{ - uint8_t command[3] = { W25N01G_INSTRUCTION_READ_STATUS_REG, reg, 0 }; - uint8_t in[3]; - - busTransfer(busDev, in, command, sizeof(command)); - - return in[2]; -} - -static void w25n01g_writeRegister(uint8_t reg, uint8_t data) -{ - uint8_t cmd[3] = { W25N01G_INSTRUCTION_WRITE_STATUS_REG, reg, data }; - busTransfer(busDev, NULL, cmd, sizeof(cmd)); -} - -static void w25n01g_deviceReset(void) -{ - w25n01g_performOneByteCommand(W25N01G_INSTRUCTION_DEVICE_RESET); - w25n01g_setTimeout(W25N01G_TIMEOUT_RESET_MS); - w25n01g_waitForReadyInternal(); - // Protection for upper 1/32 (BP[3:0] = 0101, TB=0), WP-E on; to protect bad block replacement area - // DON'T DO THIS. This will prevent writes through the bblut as well. - // w25n01g_writeRegister(busdev, W25N01G_PROT_REG, W25N01G_PROT_PB0_ENABLE|W25N01G_PROT_PB2_ENABLE|W25N01G_PROT_WP_E_ENABLE); - // No protection, WP-E off, WP-E prevents use of IO2 - w25n01g_writeRegister(W25N01G_PROT_REG, W25N01G_PROT_CLEAR); - // Buffered read mode (BUF = 1), ECC enabled (ECC = 1) - w25n01g_writeRegister(W25N01G_CONF_REG, W25N01G_CONFIG_ECC_ENABLE | W25N01G_CONFIG_BUFFER_READ_MODE); -} - -bool w25n01g_isReady(void) -{ - uint8_t status = w25n01g_readRegister(W25N01G_STAT_REG); - - // If couldBeBusy is false, don't bother to poll the flash chip for its status - couldBeBusy = couldBeBusy && ((status & W25N01G_STATUS_FLAG_BUSY) != 0); - - return !couldBeBusy; -} - -static bool w25n01g_waitForReadyInternal(void) -{ - while (!w25n01g_isReady()) { - timeMs_t now = millis(); - if (cmp32(now, timeoutAt) >= 0) { - return false; - } - } - timeoutAt = 0; - return true; -} - -bool w25n01g_waitForReady(timeMs_t timeoutMillis) -{ - w25n01g_setTimeout(timeoutMillis); - return w25n01g_waitForReadyInternal(); -} - -/** - * The flash requires this write enable command to be sent before commands that would cause - * a write like program and erase. - */ -static void w25n01g_writeEnable(void) -{ - w25n01g_performOneByteCommand(W25N01G_INSTRUCTION_WRITE_ENABLE); - - // Assume that we're about to do some writing, so the device is just about to become busy - couldBeBusy = true; -} - -bool w25n01g_detect(uint32_t chipID) -{ - switch (chipID) { - case JEDEC_ID_WINBOND_W25N01GV: - geometry.sectors = 1024; // Blocks - geometry.pagesPerSector = 64; // Pages/Blocks - geometry.pageSize = 2048; - break; - case JEDEC_ID_WINBOND_W25N02KV: - geometry.sectors = 2048; // Blocks - geometry.pagesPerSector = 64; // Pages/Blocks - geometry.pageSize = 2048; - break; - - default: - // Unsupported chip - geometry.sectors = 0; - geometry.pagesPerSector = 0; - geometry.sectorSize = 0; - geometry.totalSize = 0; - return false; - } - - geometry.flashType = FLASH_TYPE_NAND; - geometry.sectorSize = geometry.pagesPerSector * geometry.pageSize; - geometry.totalSize = geometry.sectorSize * geometry.sectors; - - /*flashPartitionSet(FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT, - W25N01G_BB_MANAGEMENT_START_BLOCK, - W25N01G_BB_MANAGEMENT_START_BLOCK + W25N01G_BB_MANAGEMENT_BLOCKS - 1);*/ - - couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be - - w25n01g_deviceReset(); - - // Upper 4MB (32 blocks * 128KB/block) will be used for bad block replacement area. - // Blocks in this area are only written through bad block LUT, - // and factory written bad block marker in unused blocks are retained. - // When a replacement block is required, - // (1) "Read BB LUT" command is used to obtain the last block mapped, - // (2) blocks after the last block is scanned for a good block, - // (3) the first good block is used for replacement, and the BB LUT is updated. - // There are only 20 BB LUT entries, and there are 32 replacement blocks. - // There will be a least chance of running out of replacement blocks. - // If it ever run out, the device becomes unusable. - - return true; -} - -/** - * Erase a sector full of bytes to all 1's at the given byte offset in the flash chip. - */ -void w25n01g_eraseSector(uint32_t address) -{ - w25n01g_waitForReadyInternal(); - w25n01g_writeEnable(); - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_BLOCK_ERASE, W25N01G_LINEAR_TO_PAGE(address)); - w25n01g_setTimeout(W25N01G_TIMEOUT_BLOCK_ERASE_MS); -} - -// W25N01G does not support full chip erase. -// Call eraseSector repeatedly. -void w25n01g_eraseCompletely(void) -{ - for (uint32_t block = 0; block < geometry.sectors; block++) { - w25n01g_eraseSector(W25N01G_BLOCK_TO_LINEAR(block)); - } -} - -static void w25n01g_programDataLoad(uint16_t columnAddress, const uint8_t *data, int length) -{ - w25n01g_waitForReadyInternal(); - - uint8_t cmd[3] = {W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; - - busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; - busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); - - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_PROGRAM_MS); -} - -static void w25n01g_randomProgramDataLoad(uint16_t columnAddress, const uint8_t *data, int length) -{ - w25n01g_waitForReadyInternal(); - - uint8_t cmd[3] = {W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; - - busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; - busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); - - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_PROGRAM_MS); -} - -static void w25n01g_programExecute(uint32_t pageAddress) -{ - w25n01g_waitForReadyInternal(); - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_PROGRAM_EXECUTE, pageAddress); - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_PROGRAM_MS); -} - -// Writes are done in three steps: -// (1) Load internal data buffer with data to write -// - We use "Random Load Program Data", as "Load Program Data" resets unused data bytes in the buffer to 0xff. -// - Each "Random Load Program Data" instruction must be accompanied by at least a single data. -// - Each "Random Load Program Data" instruction terminates at the rising of CS. -// (2) Enable write -// (3) Issue "Execute Program" -// -/* -flashfs page program behavior -- Single program never crosses page boundary. -- Except for this characteristic, it program arbitral size. -- Write address is, naturally, not a page boundary. -To cope with this behavior. -pageProgramBegin: -If buffer is dirty and programLoadAddress != address, then the last page is a partial write; -issue PAGE_PROGRAM_EXECUTE to flash buffer contents, clear dirty and record the address as programLoadAddress and programStartAddress. -Else do nothing. -pageProgramContinue: -Mark buffer as dirty. -If programLoadAddress is on page boundary, then issue PROGRAM_LOAD_DATA, else issue RANDOM_PROGRAM_LOAD_DATA. -Update programLoadAddress. -Optionally observe the programLoadAddress, and if it's on page boundary, issue PAGE_PROGRAM_EXECUTE. -pageProgramFinish: -Observe programLoadAddress. If it's on page boundary, issue PAGE_PROGRAM_EXECUTE and clear dirty, else just return. -If pageProgramContinue observes the page boundary, then do nothing(?). -*/ -bool bufferDirty = false; -bool isProgramming = false; -static uint32_t programStartAddress; -static uint32_t programLoadAddress; -static uint32_t currentPage = UINT32_MAX; - -void w25n01g_pageProgramBegin(uint32_t address) -{ - if (bufferDirty) { - if (address != programLoadAddress) { - w25n01g_waitForReadyInternal(); - isProgramming = false; - w25n01g_writeEnable(); - w25n01g_programExecute(W25N01G_LINEAR_TO_PAGE(programStartAddress)); - bufferDirty = false; - isProgramming = true; - } - } else { - programStartAddress = programLoadAddress = address; - } -} - -void w25n01g_pageProgramContinue(const uint8_t *data, int length) -{ - // Check for page boundary overrun - w25n01g_waitForReadyInternal(); - w25n01g_writeEnable(); - isProgramming = false; - if (!bufferDirty) { - w25n01g_programDataLoad(W25N01G_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); - } else { - w25n01g_randomProgramDataLoad(W25N01G_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); - } - // XXX Test if write enable is reset after each data loading. - bufferDirty = true; - programLoadAddress += length; -} - -void w25n01g_pageProgramFinish(void) -{ - if (bufferDirty && W25N01G_LINEAR_TO_COLUMN(programLoadAddress) == 0) { - currentPage = W25N01G_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written - w25n01g_programExecute(W25N01G_LINEAR_TO_PAGE(programStartAddress)); - bufferDirty = false; - isProgramming = true; - programStartAddress = programLoadAddress; - } -} - -/* - * Write bytes to a flash page. Address must not cross a page boundary. - * - * Bits can only be set to zero, not from zero back to one again. In order to set bits to 1, use the erase command. - * - * Length must be smaller than the page size. - * - * This will wait for the flash to become ready before writing begins. - * - * Datasheet indicates typical programming time is 0.8ms for 256 bytes, 0.2ms for 64 bytes, 0.05ms for 16 bytes. - * (Although the maximum possible write time is noted as 5ms). - * - * If you want to write multiple buffers (whose sum of sizes is still not more than the page size) then you can - * break this operation up into one beginProgram call, one or more continueProgram calls, and one finishProgram call. - */ -uint32_t w25n01g_pageProgram(uint32_t address, const uint8_t *data, int length) -{ - w25n01g_pageProgramBegin(address); - w25n01g_pageProgramContinue((uint8_t *)data, length); - w25n01g_pageProgramFinish(); - - return address + length; -} - -void w25n01g_flush(void) -{ - if (bufferDirty) { - currentPage = W25N01G_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written - w25n01g_programExecute(W25N01G_LINEAR_TO_PAGE(programStartAddress)); - bufferDirty = false; - isProgramming = true; - } else { - isProgramming = false; - } -} - -void w25n01g_addError(uint32_t address, uint8_t code) -{ - UNUSED(address); - UNUSED(code); -} - -/* - * Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie - * on a page boundary). - * - * Waits up to W25N01G_TIMEOUT_PAGE_READ_MS milliseconds for the flash to become ready before reading. - * - * The number of bytes actually read is returned, which can be zero if an error or timeout occurred. - */ -// Continuous read mode (BUF = 0): -// (1) "Page Data Read" command is executed for the page pointed by address -// (2) "Read Data" command is executed for bytes not requested and data are discarded -// (3) "Read Data" command is executed and data are stored directly into caller's buffer -// -// Buffered read mode (BUF = 1), non-read ahead -// (1) If currentBufferPage != requested page, then issue PAGE_DATA_READ on requested page. -// (2) Compute transferLength as smaller of remaining length and requested length. -// (3) Issue READ_DATA on column address. -// (4) Return transferLength. -int w25n01g_readBytes(uint32_t address, uint8_t *buffer, int length) -{ - uint32_t targetPage = W25N01G_LINEAR_TO_PAGE(address); - - if (currentPage != targetPage) { - if (!w25n01g_waitForReadyInternal()) { - return 0; - } - currentPage = UINT32_MAX; - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_PAGE_DATA_READ, targetPage); - if (!w25n01g_waitForReady(W25N01G_TIMEOUT_PAGE_READ_MS)) { - return 0; - } - currentPage = targetPage; - } - - uint16_t column = W25N01G_LINEAR_TO_COLUMN(address); - uint16_t transferLength; - - if (length > W25N01G_PAGE_SIZE - column) { - transferLength = W25N01G_PAGE_SIZE - column; - } else { - transferLength = length; - } - - const uint8_t cmd[4] = {W25N01G_INSTRUCTION_READ_DATA, (column >> 8) & 0xff, (column >> 0) & 0xff, 0}; - - busTransferDescriptor_t readDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = transferLength, .rxBuf = buffer, .txBuf = NULL}}; - busTransferMultiple(busDev, readDescr, ARRAYLEN(readDescr)); - - if (!w25n01g_waitForReady(W25N01G_TIMEOUT_PAGE_READ_MS)) { - return 0; - } - - // Check ECC - uint8_t statReg = w25n01g_readRegister(W25N01G_STAT_REG); - uint8_t eccCode = W25N01G_STATUS_FLAG_ECC(statReg); - switch (eccCode) { - case 0: // Successful read, no ECC correction - break; - - case 1: // Successful read with ECC correction - case 2: // Uncorrectable ECC in a single page - case 3: // Uncorrectable ECC in multiple pages - w25n01g_addError(address, eccCode); - w25n01g_deviceReset(); - break; - } - - return transferLength; -} - -int w25n01g_readExtensionBytes(uint32_t address, uint8_t *buffer, int length) -{ - if (!w25n01g_waitForReadyInternal()) { - return 0; - } - - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_PAGE_DATA_READ, W25N01G_LINEAR_TO_PAGE(address)); - - uint32_t column = 2048; - - uint8_t cmd[4]; - cmd[0] = W25N01G_INSTRUCTION_READ_DATA; - cmd[1] = (column >> 8) & 0xff; - cmd[2] = (column >> 0) & 0xff; - cmd[3] = 0; - - busTransferDescriptor_t readDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = buffer, .txBuf = NULL}}; - busTransferMultiple(busDev, readDescr, ARRAYLEN(readDescr)); - - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_READ_MS); - - return length; -} - -/** - * Fetch information about the detected flash chip layout. - * - * Can be called before calling w25n01g_init() (the result would have totalSize = 0). - */ -const flashGeometry_t* w25n01g_getGeometry(void) -{ - return &geometry; -} - -bool w25n01g_init(int flashNumToUse) -{ - busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_W25N01G, flashNumToUse, OWNER_FLASH); - if (busDev == NULL) { - return false; - } - - uint8_t in[4] = { 0 }; - uint32_t chipID; - - delay(50); // short delay required after initialisation of SPI device instance. - - busReadBuf(busDev, W25N01G_INSTRUCTION_RDID, in, sizeof(in)); - - chipID = (in[1] << 16) | (in[2] << 8) | (in[3]); - - return w25n01g_detect(chipID); -} - -#endif \ No newline at end of file diff --git a/src/main/target/NEXUSX/CMakeLists.txt b/src/main/target/NEXUSX/CMakeLists.txt new file mode 100644 index 00000000000..aa85fc2f129 --- /dev/null +++ b/src/main/target/NEXUSX/CMakeLists.txt @@ -0,0 +1,3 @@ +target_stm32f722xe(NEXUSX) +target_stm32f722xe(NEXUSX_9SERVOS) +target_stm32f722xe(NEXUSX_NOI2C) diff --git a/src/main/target/NEXUSX/README.md b/src/main/target/NEXUSX/README.md new file mode 100644 index 00000000000..df3227fd859 --- /dev/null +++ b/src/main/target/NEXUSX/README.md @@ -0,0 +1,61 @@ +Radiomaster NEXUS X and NEXUS XR +================================ + +Flight controllers originally designed for Helicopters using Rotorflight. +Based on STM32F722RET6. Both NEXUS X and XR share the same targets in iNav. + +Built-in periperals +------------------- + +Both models contain a ICM42688P IMU, a SPL06 barometer and a W25N02KVZEIR blackbox memory chip. + +The NEXUS XR also contains a serial ELRS receiver based on the RP4TD-M using ESP32 and two SX1281. +It is connected to the main STM32F7 Flight Controller on UART5. +None of the external connections route to the receiver, they are all connected to the STM32F7 Flight Controller. +The receiver can be disabled using USER1, which controls a pinio on pin PC8. + +Pin configuration +----------------- + +All pin orders are from left to right, when looking at the connector on the flight controller. +Note that the order is incorrect on radiomaster's website, it has RX and TX swapped. + +| Marking on the case | NEXUSX | NEXUSX_9SERVOS | NEXUSX_NOI2C | +|---------------------|-------------------------------------------------------|-------------------------------------------------------|-------------------------------------------------------| +| S1 | Output S1 | Output S1 | Output S1 | +| S2 | Output S2 | Output S2 | Output S2 | +| S3 | Output S3 | Output S3 | Output S3 | +| TAIL | Output S4 | Output S4 | Output S4 | +| ESC | Output S5 | Output S5 | Output S5 | +| RPM | Output S6 | Output S6 | Output S6 | +| TLM | Output S7 | Output S7 | Output S7 | +| AUX | UART1 TX | Output S8 | Output S8 | +| SBUS | UART1 RX | Output S9 | Output S9 | +| A | UART4
pin order:
TX, RX, 5V, GND | UART4
pin order:
TX, RX, 5V, GND | UART4
pin order:
TX, RX, 5V, GND | +| B | UART6
pin order:
TX, RX, 5V, GND | UART6
pin order:
TX, RX, 5V, GND | UART6
pin order:
TX, RX, 5V, GND | +| C | I2C
pin order:
SCL, SDA, 5V, GND | I2C
pin order:
SCL, SDA, 5V, GND | UART3
pin order:
TX, RX, 5V, GND | +| EXT-V | battery voltage
max 60V
pin order:
Vbat, GND | battery voltage
max 60V
pin order:
Vbat, GND | battery voltage
max 60V
pin order:
Vbat, GND | +| built-in ELRS | UART5 | UART5 | UART5 | +Hardware layout +--------------- + + +| Marking on the case | STM32 pin | Servo | UART | I2C | +|---------------------|-----------|------------------------------:|---------------:|---------:| +| S1 | PB4 | TIM3CH1 | n/a | n/a | +| S2 | PB5 | TIM3CH2 | n/a | n/a | +| S3 | PB0 | TIM3CH3 | n/a | n/a | +| TAIL | PA15 | TIM2CH1 | n/a | n/a | +| ESC | PA9 | TIM1CH2 | UART1 TX | n/a | +| RPM | PA2 | TIM2CH3
TIM5CH3
TIM9CH1 | UART2 TX | n/a | +| TLM | PA3 | TIM2CH4
TIM5CH4
TIM9CH2 | UART2 RX | n/a | +| AUX | PB6 | TIM4CH1 | UART1 TX | I2C1 SCL | +| SBUS | PB7 | TIM4CH2 | UART1 RX | I2C1 SDA | +| A | PA1/PB0 | TIM1
TIM5 | UART2
UART4 | n/a | +| B | PC7/PC6 | TIM3
TIM8 | UART6 | n/a | +| C | PB11/PB10 | TIM2 | UART3 | I2C2 | +| EXT-V | PC0 | n/a | n/a | n/a | +| built-in ELRS | PC12/PD2 | n/a | UART5 | n/a | + +The pinout is extremely similar to the F7C reference design from Rotorflight. +https://github.com/rotorflight/rotorflight-ref-design/blob/master/Reference-Design-F7C.md \ No newline at end of file diff --git a/src/main/target/NEXUSX/config.c b/src/main/target/NEXUSX/config.c new file mode 100644 index 00000000000..09970cef5c1 --- /dev/null +++ b/src/main/target/NEXUSX/config.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/pwm_mapping.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/NEXUSX/target.c b/src/main/target/NEXUSX/target.c new file mode 100644 index 00000000000..dc57e7bcec2 --- /dev/null +++ b/src/main/target/NEXUSX/target.c @@ -0,0 +1,45 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "target.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "S1" + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "S2" + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "S3" + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "TAIL" + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "ESC" + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "RPM", clashes with UART2 TX + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "TLM", clashes with UART2 RX + +#if defined(NEXUSX_9SERVOS) || defined(NEXUSX_NOI2C) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "AUX", clashes with UART1 TX and I2C1 SCL + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "SBUS", clashes with UART1 RX and I2C1 SDA +#endif +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEXUSX/target.h b/src/main/target/NEXUSX/target.h new file mode 100644 index 00000000000..a01f973fd1b --- /dev/null +++ b/src/main/target/NEXUSX/target.h @@ -0,0 +1,170 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "F7C5" + +#define USBD_PRODUCT_STRING "NEXUSX" + +#define LED0 PC10 +#define LED1 PC11 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 // is actually ICM42688P +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_EXTI_PIN PB8 +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_3 +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + +//#define USE_I2C_DEVICE_1 // clashes with UART1 +//#define I2C1_SCL PB6 +//#define I2C1_SDA PB7 + +#if defined(NEXUSX) || defined(NEXUSX_9SERVOS) +#define USE_I2C_DEVICE_2 // clashes with UART3 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +#define DEFAULT_I2C BUS_I2C2 +#else +#define DEFAULT_I2C BUS_I2C3 +#endif + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C3 +#define USE_BARO_SPL06 + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C + +#define PITOT_I2C_BUS DEFAULT_I2C + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C + +// *************** SPI2 Blackbox ******************* +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_W25N02K +#define W25N02K_SPI_BUS BUS_SPI2 +#define W25N02K_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#ifdef NEXUSX +#define USE_UART1 // clashes with I2C1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 // pin labelled "SBUS" +#endif + +//#define USE_UART2 // clashes with 2 servo outputs +//#define UART2_TX_PIN PA2 // pin labelled as "RPM" +//#define UART2_RX_PIN PA3 // pin labelled as "TLM" + +#ifdef NEXUSX_NOI2C +#define USE_UART3 +// port labelled "C" +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#endif + +#define USE_UART4 +// port labelled "A" +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +// port for NEXUS XR internal ELRS receiver +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +// port labelled "B" +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#if defined(NEXUSX) +#define SERIAL_PORT_COUNT 5 +#elif defined(NEXUSX_9SERVOS) +#define SERIAL_PORT_COUNT 4 +#elif defined(NEXUSX_NOI2C) +#define SERIAL_PORT_COUNT 5 +#endif + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 + +#define SENSORS_SET (SENSOR_ACC|SENSOR_BARO) + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC0 + +#define VBAT_ADC_CHANNEL ADC_CHN_3 // port labelled "EXT-V" +//BEC ADC is ADC_CHN_2 +//BUS ADC is ADC_CHN_1 + +#define VBAT_SCALE_DEFAULT 2474 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC8 // enable pin for internal ELRS receiver +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED // turn on by default + +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_TX_PROF_SEL) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#if defined(NEXUSX) +#define MAX_PWM_OUTPUT_PORTS 7 +#elif defined(NEXUSX_9SERVOS) || defined(NEXUSX_NOI2C) +#define MAX_PWM_OUTPUT_PORTS 9 +#endif + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR +#define USE_SMARTPORT_MASTER // no internal current sensor, enable SMARTPORT_MASTER so external ones can be used diff --git a/src/main/target/ORBITH743/target.h b/src/main/target/ORBITH743/target.h index 2c754003586..674240b85b0 100644 --- a/src/main/target/ORBITH743/target.h +++ b/src/main/target/ORBITH743/target.h @@ -125,12 +125,12 @@ #define MAX7456_CS_PIN PE6 // *************** FLASH *************************** -#define W25N01G_SPI_BUS BUS_SPI3 -#define W25N01G_CS_PIN PD3 +#define W25N02K_SPI_BUS BUS_SPI3 +#define W25N02K_CS_PIN PD3 #define USE_BLACKBOX #define USE_FLASHFS -#define USE_FLASH_W25N01G +#define USE_FLASH_W25N02K #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT // *************** Baro/Mag ********************* diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 69a2146fc86..68ec100ac12 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -417,7 +417,11 @@ #endif #if defined(USE_FLASH_W25N01G) - BUSDEV_REGISTER_SPI(busdev_w25n01g, DEVHW_W25N01G, W25N01G_SPI_BUS, W25N01G_CS_PIN, NONE, DEVFLAGS_NONE, 0); + BUSDEV_REGISTER_SPI(busdev_w25n01g, DEVHW_W25N, W25N01G_SPI_BUS, W25N01G_CS_PIN, NONE, DEVFLAGS_NONE, 0); +#endif + +#if defined(USE_FLASH_W25N02K) + BUSDEV_REGISTER_SPI(busdev_w25n02k, DEVHW_W25N, W25N02K_SPI_BUS, W25N02K_CS_PIN, NONE, DEVFLAGS_NONE, 0); #endif #if defined(USE_SDCARD) && defined(USE_SDCARD_SPI) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index aaf6ad22157..5ccbb6b4ad6 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -464,7 +464,8 @@ def writeTargetH(folder, map): 'W25M', 'W25M02G', 'W25M512', - 'W25N01G' + 'W25N01G', + 'W25N02K', ] file.write("#define USE_FLASHFS\n") file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n")