diff --git a/make/source.mk b/make/source.mk
index a129fb773e..b940201ccb 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -374,6 +374,7 @@ ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
SRC += \
drivers/flash.c \
drivers/flash_m25p16.c \
+ drivers/flash_w25m.c \
io/flashfs.c \
pg/flash.c \
$(MSC_SRC)
diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c
index cc09af55aa..9095c372d2 100644
--- a/src/main/drivers/flash.c
+++ b/src/main/drivers/flash.c
@@ -27,6 +27,7 @@
#include "flash.h"
#include "flash_impl.h"
#include "flash_m25p16.h"
+#include "flash_w25m.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/time.h"
@@ -83,6 +84,12 @@ bool flashInit(const flashConfig_t *flashConfig)
}
#endif
+#ifdef USE_FLASH_W25M
+ if (w25m_detect(&flashDevice, chipID)) {
+ return true;
+ }
+#endif
+
spiPreInitCs(flashConfig->csTag);
return false;
diff --git a/src/main/drivers/flash_w25m.c b/src/main/drivers/flash_w25m.c
new file mode 100644
index 0000000000..4752867b4c
--- /dev/null
+++ b/src/main/drivers/flash_w25m.c
@@ -0,0 +1,237 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+
+/*
+ * Winbond W25M series stacked die flash driver.
+ * Handles homogeneous stack of identical dies by calling die drivers.
+ *
+ * Author: jflyper
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#ifdef USE_FLASH_W25M
+
+#include "common/maths.h"
+#include "drivers/bus_spi.h"
+#include "drivers/flash.h"
+#include "drivers/flash_impl.h"
+#include "drivers/io.h"
+#include "drivers/time.h"
+
+#include "flash_m25p16.h"
+#include "flash_w25m.h"
+
+#include "pg/flash.h"
+
+#define W25M_INSTRUCTION_SOFTWARE_DIE_SELECT 0xC2
+
+#define JEDEC_ID_WINBOND_W25M512 0xEF7119 // W25Q256 x 2
+
+static const flashVTable_t w25m_vTable;
+
+#define MAX_DIE_COUNT 2
+
+static flashDevice_t dieDevice[MAX_DIE_COUNT];
+
+static int dieCount;
+static uint32_t dieSize;
+
+static void w25m_dieSelect(busDevice_t *busdev, int die)
+{
+ static int activeDie = -1;
+
+ if (activeDie == die) {
+ return;
+ }
+
+ uint8_t command[2] = { W25M_INSTRUCTION_SOFTWARE_DIE_SELECT, die };
+
+ spiBusTransfer(busdev, command, NULL, 2);
+
+ activeDie = die;
+}
+
+static bool w25m_isReady(flashDevice_t *fdevice)
+{
+ UNUSED(fdevice);
+
+ for (int die = 0 ; die < dieCount ; die++) {
+ if (dieDevice[die].couldBeBusy) {
+ w25m_dieSelect(fdevice->busdev, die);
+ if (!dieDevice[die].vTable->isReady(&dieDevice[die])) {
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+static bool w25m_waitForReady(flashDevice_t *fdevice, uint32_t timeoutMillis)
+{
+ uint32_t time = millis();
+ while (!w25m_isReady(fdevice)) {
+ if (millis() - time > timeoutMillis) {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
+{
+
+ switch (chipID) {
+ case JEDEC_ID_WINBOND_W25M512:
+ // W25Q256 x 2
+ dieCount = 2;
+
+ for (int die = 0 ; die < dieCount ; die++) {
+ w25m_dieSelect(fdevice->busdev, die);
+ dieDevice[die].busdev = fdevice->busdev;
+ m25p16_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25Q256);
+ }
+
+ fdevice->geometry.flashType = FLASH_TYPE_NOR;
+ break;
+
+ default:
+ // Not a valid W25M series device
+ fdevice->geometry.sectors = 0;
+ fdevice->geometry.pagesPerSector = 0;
+ fdevice->geometry.sectorSize = 0;
+ fdevice->geometry.totalSize = 0;
+ return false;
+ }
+
+ fdevice->geometry.sectors = dieDevice[0].geometry.sectors;
+ fdevice->geometry.sectorSize = dieDevice[0].geometry.sectorSize;
+ fdevice->geometry.pagesPerSector = dieDevice[0].geometry.pagesPerSector;
+ fdevice->geometry.pageSize = dieDevice[0].geometry.pageSize;
+ dieSize = dieDevice[0].geometry.totalSize;
+ fdevice->geometry.totalSize = dieSize * dieCount;
+ fdevice->vTable = &w25m_vTable;
+
+ return true;
+}
+
+void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address)
+{
+ int dieNumber = address / dieSize;
+
+ w25m_dieSelect(fdevice->busdev, dieNumber);
+
+ dieDevice[dieNumber].vTable->eraseSector(&dieDevice[dieNumber], address % dieSize);
+}
+
+void w25m_eraseCompletely(flashDevice_t *fdevice)
+{
+ for (int dieNumber = 0 ; dieNumber < dieCount ; dieNumber++) {
+ w25m_dieSelect(fdevice->busdev, dieNumber);
+ dieDevice[dieNumber].vTable->eraseCompletely(&dieDevice[dieNumber]);
+ }
+}
+
+static uint32_t currentWriteAddress;
+static int currentWriteDie;
+
+void w25m_pageProgramBegin(flashDevice_t *fdevice, uint32_t address)
+{
+ UNUSED(fdevice);
+
+ currentWriteDie = address / dieSize;
+ w25m_dieSelect(fdevice->busdev, currentWriteDie);
+ currentWriteAddress = address % dieSize;
+ dieDevice[currentWriteDie].vTable->pageProgramBegin(&dieDevice[currentWriteDie], currentWriteAddress);
+}
+
+void w25m_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length)
+{
+ UNUSED(fdevice);
+
+ dieDevice[currentWriteDie].vTable->pageProgramContinue(&dieDevice[currentWriteDie], data, length);
+}
+
+void w25m_pageProgramFinish(flashDevice_t *fdevice)
+{
+ UNUSED(fdevice);
+
+ dieDevice[currentWriteDie].vTable->pageProgramFinish(&dieDevice[currentWriteDie]);
+}
+
+void w25m_pageProgram(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, int length)
+{
+ w25m_pageProgramBegin(fdevice, address);
+
+ w25m_pageProgramContinue(fdevice, data, length);
+
+ w25m_pageProgramFinish(fdevice);
+}
+
+int w25m_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, int length)
+{
+ int rlen; // remaining length
+ int tlen; // transfer length for a round
+ int rbytes;
+
+ // Divide a read that spans multiple dies into two.
+ // The loop is executed twice at the most for decent 'length'.
+
+ for (rlen = length; rlen; rlen -= tlen) {
+ int dieNumber = address / dieSize;
+ uint32_t dieAddress = address % dieSize;
+ tlen = MIN(dieAddress + rlen, dieSize) - dieAddress;
+
+ w25m_dieSelect(fdevice->busdev, dieNumber);
+
+ rbytes = dieDevice[dieNumber].vTable->readBytes(&dieDevice[dieNumber], dieAddress, buffer, tlen);
+
+ if (!rbytes) {
+ return 0;
+ }
+
+ address += tlen;
+ buffer += tlen;
+ }
+ return length;
+}
+
+const flashGeometry_t* w25m_getGeometry(flashDevice_t *fdevice)
+{
+ return &fdevice->geometry;
+}
+
+static const flashVTable_t w25m_vTable = {
+ .isReady = w25m_isReady,
+ .waitForReady = w25m_waitForReady,
+ .eraseSector = w25m_eraseSector,
+ .eraseCompletely = w25m_eraseCompletely,
+ .pageProgramBegin = w25m_pageProgramBegin,
+ .pageProgramContinue = w25m_pageProgramContinue,
+ .pageProgramFinish = w25m_pageProgramFinish,
+ .pageProgram = w25m_pageProgram,
+ .readBytes = w25m_readBytes,
+ .getGeometry = w25m_getGeometry,
+};
+#endif
diff --git a/src/main/drivers/flash_w25m.h b/src/main/drivers/flash_w25m.h
new file mode 100644
index 0000000000..2652f6654b
--- /dev/null
+++ b/src/main/drivers/flash_w25m.h
@@ -0,0 +1,22 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include "flash_impl.h"
+
+bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID);
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index f7e03a1cea..8e8e620648 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -132,6 +132,7 @@
// Globally configure flashfs and drivers for various flash chips
#define USE_FLASHFS
#define USE_FLASH_M25P16
+#define USE_FLASH_W25M512
#if defined(OMNIBUSF4SD)
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h
index 9c42755d0d..777c90b497 100644
--- a/src/main/target/common_fc_post.h
+++ b/src/main/target/common_fc_post.h
@@ -120,6 +120,11 @@
#define USE_32K_CAPABLE_GYRO
#endif
+#if defined(USE_FLASH_W25M512)
+#define USE_FLASH_W25M
+#define USE_FLASH_M25P16
+#endif
+
#if defined(USE_FLASH_M25P16)
#define USE_FLASH
#endif