Merge remote-tracking branch 'upstream/develop'

25 files changed, 3390 insertions, 291 deletions

diff --git a/platforms/chibios/drivers/eeprom/eeprom_stm32.c b/platforms/chibios/drivers/eeprom/eeprom_stm32.c
new file mode 100644
index 0000000000..1a354dc213
--- /dev/null
+++ b/platforms/chibios/drivers/eeprom/eeprom_stm32.c
@@ -0,0 +1,629 @@
+/*
+ * This software is experimental and a work in progress.
+ * Under no circumstances should these files be used in relation to any critical system(s).
+ * Use of these files is at your own risk.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+ * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * This files are free to use from http://engsta.com/stm32-flash-memory-eeprom-emulator/ by
+ * Artur F.
+ *
+ * Modifications for QMK and STM32F303 by Yiancar
+ * Modifications to add flash wear leveling by Ilya Zhuravlev
+ * Modifications to increase flash density by Don Kjer
+ */
+
+#include <stdio.h>
+#include <stdbool.h>
+#include "util.h"
+#include "debug.h"
+#include "eeprom_stm32.h"
+#include "flash_stm32.h"
+
+/*
+ * We emulate eeprom by writing a snapshot compacted view of eeprom contents,
+ * followed by a write log of any change since that snapshot:
+ *
+ * === SIMULATED EEPROM CONTENTS ===
+ *
+ * ┌─ Compacted ┬ Write Log ─┐
+ * │............│[BYTE][BYTE]│
+ * │FFFF....FFFF│[WRD0][WRD1]│
+ * │FFFFFFFFFFFF│[WORD][NEXT]│
+ * │....FFFFFFFF│[BYTE][WRD0]│
+ * ├────────────┼────────────┤
+ * └──PAGE_BASE │            │
+ *    PAGE_LAST─┴─WRITE_BASE │
+ *                WRITE_LAST ┘
+ *
+ * Compacted contents are the 1's complement of the actual EEPROM contents.
+ * e.g. An 'FFFF' represents a '0000' value.
+ *
+ * The size of the 'compacted' area is equal to the size of the 'emulated' eeprom.
+ * The size of the compacted-area and write log are configurable, and the combined
+ * size of Compacted + WriteLog is a multiple FEE_PAGE_SIZE, which is MCU dependent.
+ * Simulated Eeprom contents are located at the end of available flash space.
+ *
+ * The following configuration defines can be set:
+ *
+ * FEE_PAGE_COUNT   # Total number of pages to use for eeprom simulation (Compact + Write log)
+ * FEE_DENSITY_BYTES   # Size of simulated eeprom. (Defaults to half the space allocated by FEE_PAGE_COUNT)
+ * NOTE: The current implementation does not include page swapping,
+ * and FEE_DENSITY_BYTES will consume that amount of RAM as a cached view of actual EEPROM contents.
+ *
+ * The maximum size of FEE_DENSITY_BYTES is currently 16384. The write log size equals
+ * FEE_PAGE_COUNT * FEE_PAGE_SIZE - FEE_DENSITY_BYTES.
+ * The larger the write log, the less frequently the compacted area needs to be rewritten.
+ *
+ *
+ * *** General Algorithm ***
+ *
+ * During initialization:
+ * The contents of the Compacted-flash area are loaded and the 1's complement value
+ * is cached into memory (e.g. 0xFFFF in Flash represents 0x0000 in cache).
+ * Write log entries are processed until a 0xFFFF is reached.
+ * Each log entry updates a byte or word in the cache.
+ *
+ * During reads:
+ * EEPROM contents are given back directly from the cache in memory.
+ *
+ * During writes:
+ * The contents of the cache is updated first.
+ * If the Compacted-flash area corresponding to the write address is unprogrammed, the 1's complement of the value is written directly into Compacted-flash
+ * Otherwise:
+ * If the write log is full, erase both the Compacted-flash area and the Write log, then write cached contents to the Compacted-flash area.
+ * Otherwise a Write log entry is constructed and appended to the next free position in the Write log.
+ *
+ *
+ * *** Write Log Structure ***
+ *
+ * Write log entries allow for optimized byte writes to addresses below 128. Writing 0 or 1 words are also optimized when word-aligned.
+ *
+ * === WRITE LOG ENTRY FORMATS ===
+ *
+ * ╔═══ Byte-Entry ══╗
+ * ║0XXXXXXX║YYYYYYYY║
+ * ║ └──┬──┘║└──┬───┘║
+ * ║ Address║ Value  ║
+ * ╚════════╩════════╝
+ * 0 <= Address < 0x80 (128)
+ *
+ * ╔ Word-Encoded 0 ╗
+ * ║100XXXXXXXXXXXXX║
+ * ║  │└─────┬─────┘║
+ * ║  │Address >> 1 ║
+ * ║  └── Value: 0  ║
+ * ╚════════════════╝
+ * 0 <= Address <= 0x3FFE (16382)
+ *
+ * ╔ Word-Encoded 1 ╗
+ * ║101XXXXXXXXXXXXX║
+ * ║  │└─────┬─────┘║
+ * ║  │Address >> 1 ║
+ * ║  └── Value: 1  ║
+ * ╚════════════════╝
+ * 0 <= Address <= 0x3FFE (16382)
+ *
+ * ╔═══ Reserved ═══╗
+ * ║110XXXXXXXXXXXXX║
+ * ╚════════════════╝
+ *
+ * ╔═══════════ Word-Next ═══════════╗
+ * ║111XXXXXXXXXXXXX║YYYYYYYYYYYYYYYY║
+ * ║   └─────┬─────┘║└───────┬──────┘║
+ * ║(Address-128)>>1║     ~Value     ║
+ * ╚════════════════╩════════════════╝
+ * (  0 <= Address <  0x0080 (128): Reserved)
+ * 0x80 <= Address <= 0x3FFE (16382)
+ *
+ * Write Log entry ranges:
+ * 0x0000 ... 0x7FFF - Byte-Entry;     address is (Entry & 0x7F00) >> 4; value is (Entry & 0xFF)
+ * 0x8000 ... 0x9FFF - Word-Encoded 0; address is (Entry & 0x1FFF) << 1; value is 0
+ * 0xA000 ... 0xBFFF - Word-Encoded 1; address is (Entry & 0x1FFF) << 1; value is 1
+ * 0xC000 ... 0xDFFF - Reserved
+ * 0xE000 ... 0xFFBF - Word-Next;      address is (Entry & 0x1FFF) << 1 + 0x80; value is ~(Next_Entry)
+ * 0xFFC0 ... 0xFFFE - Reserved
+ * 0xFFFF            - Unprogrammed
+ *
+ */
+
+#include "eeprom_stm32_defs.h"
+/* These bits are used for optimizing encoding of bytes, 0 and 1 */
+#define FEE_WORD_ENCODING 0x8000
+#define FEE_VALUE_NEXT 0x6000
+#define FEE_VALUE_RESERVED 0x4000
+#define FEE_VALUE_ENCODED 0x2000
+#define FEE_BYTE_RANGE 0x80
+
+/* Flash word value after erase */
+#define FEE_EMPTY_WORD ((uint16_t)0xFFFF)
+
+#if !defined(FEE_PAGE_SIZE) || !defined(FEE_PAGE_COUNT) || !defined(FEE_MCU_FLASH_SIZE) || !defined(FEE_PAGE_BASE_ADDRESS)
+#    error "not implemented."
+#endif
+
+/* In-memory contents of emulated eeprom for faster access */
+/* *TODO: Implement page swapping */
+static uint16_t WordBuf[FEE_DENSITY_BYTES / 2];
+static uint8_t *DataBuf = (uint8_t *)WordBuf;
+
+/* Pointer to the first available slot within the write log */
+static uint16_t *empty_slot;
+
+// #define DEBUG_EEPROM_OUTPUT
+
+/*
+ * Debug print utils
+ */
+
+#if defined(DEBUG_EEPROM_OUTPUT)
+
+#    define debug_eeprom debug_enable
+#    define eeprom_println(s) println(s)
+#    define eeprom_printf(fmt, ...) xprintf(fmt, ##__VA_ARGS__);
+
+#else /* NO_DEBUG */
+
+#    define debug_eeprom false
+#    define eeprom_println(s)
+#    define eeprom_printf(fmt, ...)
+
+#endif /* NO_DEBUG */
+
+void print_eeprom(void) {
+#ifndef NO_DEBUG
+    int empty_rows = 0;
+    for (uint16_t i = 0; i < FEE_DENSITY_BYTES; i++) {
+        if (i % 16 == 0) {
+            if (i >= FEE_DENSITY_BYTES - 16) {
+                /* Make sure we display the last row */
+                empty_rows = 0;
+            }
+            /* Check if this row is uninitialized */
+            ++empty_rows;
+            for (uint16_t j = 0; j < 16; j++) {
+                if (DataBuf[i + j]) {
+                    empty_rows = 0;
+                    break;
+                }
+            }
+            if (empty_rows > 1) {
+                /* Repeat empty row */
+                if (empty_rows == 2) {
+                    /* Only display the first repeat empty row */
+                    println("*");
+                }
+                i += 15;
+                continue;
+            }
+            xprintf("%04x", i);
+        }
+        if (i % 8 == 0) print(" ");
+
+        xprintf(" %02x", DataBuf[i]);
+        if ((i + 1) % 16 == 0) {
+            println("");
+        }
+    }
+#endif
+}
+
+uint16_t EEPROM_Init(void) {
+    /* Load emulated eeprom contents from compacted flash into memory */
+    uint16_t *src  = (uint16_t *)FEE_COMPACTED_BASE_ADDRESS;
+    uint16_t *dest = (uint16_t *)DataBuf;
+    for (; src < (uint16_t *)FEE_COMPACTED_LAST_ADDRESS; ++src, ++dest) {
+        *dest = ~*src;
+    }
+
+    if (debug_eeprom) {
+        println("EEPROM_Init Compacted Pages:");
+        print_eeprom();
+        println("EEPROM_Init Write Log:");
+    }
+
+    /* Replay write log */
+    uint16_t *log_addr;
+    for (log_addr = (uint16_t *)FEE_WRITE_LOG_BASE_ADDRESS; log_addr < (uint16_t *)FEE_WRITE_LOG_LAST_ADDRESS; ++log_addr) {
+        uint16_t address = *log_addr;
+        if (address == FEE_EMPTY_WORD) {
+            break;
+        }
+        /* Check for lowest 128-bytes optimization */
+        if (!(address & FEE_WORD_ENCODING)) {
+            uint8_t bvalue = (uint8_t)address;
+            address >>= 8;
+            DataBuf[address] = bvalue;
+            eeprom_printf("DataBuf[0x%02x] = 0x%02x;\n", address, bvalue);
+        } else {
+            uint16_t wvalue;
+            /* Check if value is in next word */
+            if ((address & FEE_VALUE_NEXT) == FEE_VALUE_NEXT) {
+                /* Read value from next word */
+                if (++log_addr >= (uint16_t *)FEE_WRITE_LOG_LAST_ADDRESS) {
+                    break;
+                }
+                wvalue = ~*log_addr;
+                if (!wvalue) {
+                    eeprom_printf("Incomplete write at log_addr: 0x%04lx;\n", (uint32_t)log_addr);
+                    /* Possibly incomplete write.  Ignore and continue */
+                    continue;
+                }
+                address &= 0x1FFF;
+                address <<= 1;
+                /* Writes to addresses less than 128 are byte log entries */
+                address += FEE_BYTE_RANGE;
+            } else {
+                /* Reserved for future use */
+                if (address & FEE_VALUE_RESERVED) {
+                    eeprom_printf("Reserved encoded value at log_addr: 0x%04lx;\n", (uint32_t)log_addr);
+                    continue;
+                }
+                /* Optimization for 0 or 1 values. */
+                wvalue = (address & FEE_VALUE_ENCODED) >> 13;
+                address &= 0x1FFF;
+                address <<= 1;
+            }
+            if (address < FEE_DENSITY_BYTES) {
+                eeprom_printf("DataBuf[0x%04x] = 0x%04x;\n", address, wvalue);
+                *(uint16_t *)(&DataBuf[address]) = wvalue;
+            } else {
+                eeprom_printf("DataBuf[0x%04x] cannot be set to 0x%04x [BAD ADDRESS]\n", address, wvalue);
+            }
+        }
+    }
+
+    empty_slot = log_addr;
+
+    if (debug_eeprom) {
+        println("EEPROM_Init Final DataBuf:");
+        print_eeprom();
+    }
+
+    return FEE_DENSITY_BYTES;
+}
+
+/* Clear flash contents (doesn't touch in-memory DataBuf) */
+static void eeprom_clear(void) {
+    FLASH_Unlock();
+
+    for (uint16_t page_num = 0; page_num < FEE_PAGE_COUNT; ++page_num) {
+        eeprom_printf("FLASH_ErasePage(0x%04lx)\n", (uint32_t)(FEE_PAGE_BASE_ADDRESS + (page_num * FEE_PAGE_SIZE)));
+        FLASH_ErasePage(FEE_PAGE_BASE_ADDRESS + (page_num * FEE_PAGE_SIZE));
+    }
+
+    FLASH_Lock();
+
+    empty_slot = (uint16_t *)FEE_WRITE_LOG_BASE_ADDRESS;
+    eeprom_printf("eeprom_clear empty_slot: 0x%08lx\n", (uint32_t)empty_slot);
+}
+
+/* Erase emulated eeprom */
+void EEPROM_Erase(void) {
+    eeprom_println("EEPROM_Erase");
+    /* Erase compacted pages and write log */
+    eeprom_clear();
+    /* re-initialize to reset DataBuf */
+    EEPROM_Init();
+}
+
+/* Compact write log */
+static uint8_t eeprom_compact(void) {
+    /* Erase compacted pages and write log */
+    eeprom_clear();
+
+    FLASH_Unlock();
+
+    FLASH_Status final_status = FLASH_COMPLETE;
+
+    /* Write emulated eeprom contents from memory to compacted flash */
+    uint16_t *src  = (uint16_t *)DataBuf;
+    uintptr_t dest = FEE_COMPACTED_BASE_ADDRESS;
+    uint16_t  value;
+    for (; dest < FEE_COMPACTED_LAST_ADDRESS; ++src, dest += 2) {
+        value = *src;
+        if (value) {
+            eeprom_printf("FLASH_ProgramHalfWord(0x%04lx, 0x%04x)\n", (uint32_t)dest, ~value);
+            FLASH_Status status = FLASH_ProgramHalfWord(dest, ~value);
+            if (status != FLASH_COMPLETE) final_status = status;
+        }
+    }
+
+    FLASH_Lock();
+
+    if (debug_eeprom) {
+        println("eeprom_compacted:");
+        print_eeprom();
+    }
+
+    return final_status;
+}
+
+static uint8_t eeprom_write_direct_entry(uint16_t Address) {
+    /* Check if we can just write this directly to the compacted flash area */
+    uintptr_t directAddress = FEE_COMPACTED_BASE_ADDRESS + (Address & 0xFFFE);
+    if (*(uint16_t *)directAddress == FEE_EMPTY_WORD) {
+        /* Write the value directly to the compacted area without a log entry */
+        uint16_t value = ~*(uint16_t *)(&DataBuf[Address & 0xFFFE]);
+        /* Early exit if a write isn't needed */
+        if (value == FEE_EMPTY_WORD) return FLASH_COMPLETE;
+
+        FLASH_Unlock();
+
+        eeprom_printf("FLASH_ProgramHalfWord(0x%08lx, 0x%04x) [DIRECT]\n", (uint32_t)directAddress, value);
+        FLASH_Status status = FLASH_ProgramHalfWord(directAddress, value);
+
+        FLASH_Lock();
+        return status;
+    }
+    return 0;
+}
+
+static uint8_t eeprom_write_log_word_entry(uint16_t Address) {
+    FLASH_Status final_status = FLASH_COMPLETE;
+
+    uint16_t value = *(uint16_t *)(&DataBuf[Address]);
+    eeprom_printf("eeprom_write_log_word_entry(0x%04x): 0x%04x\n", Address, value);
+
+    /* MSB signifies the lowest 128-byte optimization is not in effect */
+    uint16_t encoding = FEE_WORD_ENCODING;
+    uint8_t  entry_size;
+    if (value <= 1) {
+        encoding |= value << 13;
+        entry_size = 2;
+    } else {
+        encoding |= FEE_VALUE_NEXT;
+        entry_size = 4;
+        /* Writes to addresses less than 128 are byte log entries */
+        Address -= FEE_BYTE_RANGE;
+    }
+
+    /* if we can't find an empty spot, we must compact emulated eeprom */
+    if (empty_slot > (uint16_t *)(FEE_WRITE_LOG_LAST_ADDRESS - entry_size)) {
+        /* compact the write log into the compacted flash area */
+        return eeprom_compact();
+    }
+
+    /* Word log writes should be word-aligned.  Take back a bit */
+    Address >>= 1;
+    Address |= encoding;
+
+    /* ok we found a place let's write our data */
+    FLASH_Unlock();
+
+    /* address */
+    eeprom_printf("FLASH_ProgramHalfWord(0x%08lx, 0x%04x)\n", (uint32_t)empty_slot, Address);
+    final_status = FLASH_ProgramHalfWord((uintptr_t)empty_slot++, Address);
+
+    /* value */
+    if (encoding == (FEE_WORD_ENCODING | FEE_VALUE_NEXT)) {
+        eeprom_printf("FLASH_ProgramHalfWord(0x%08lx, 0x%04x)\n", (uint32_t)empty_slot, ~value);
+        FLASH_Status status = FLASH_ProgramHalfWord((uintptr_t)empty_slot++, ~value);
+        if (status != FLASH_COMPLETE) final_status = status;
+    }
+
+    FLASH_Lock();
+
+    return final_status;
+}
+
+static uint8_t eeprom_write_log_byte_entry(uint16_t Address) {
+    eeprom_printf("eeprom_write_log_byte_entry(0x%04x): 0x%02x\n", Address, DataBuf[Address]);
+
+    /* if couldn't find an empty spot, we must compact emulated eeprom */
+    if (empty_slot >= (uint16_t *)FEE_WRITE_LOG_LAST_ADDRESS) {
+        /* compact the write log into the compacted flash area */
+        return eeprom_compact();
+    }
+
+    /* ok we found a place let's write our data */
+    FLASH_Unlock();
+
+    /* Pack address and value into the same word */
+    uint16_t value = (Address << 8) | DataBuf[Address];
+
+    /* write to flash */
+    eeprom_printf("FLASH_ProgramHalfWord(0x%08lx, 0x%04x)\n", (uint32_t)empty_slot, value);
+    FLASH_Status status = FLASH_ProgramHalfWord((uintptr_t)empty_slot++, value);
+
+    FLASH_Lock();
+
+    return status;
+}
+
+uint8_t EEPROM_WriteDataByte(uint16_t Address, uint8_t DataByte) {
+    /* if the address is out-of-bounds, do nothing */
+    if (Address >= FEE_DENSITY_BYTES) {
+        eeprom_printf("EEPROM_WriteDataByte(0x%04x, 0x%02x) [BAD ADDRESS]\n", Address, DataByte);
+        return FLASH_BAD_ADDRESS;
+    }
+
+    /* if the value is the same, don't bother writing it */
+    if (DataBuf[Address] == DataByte) {
+        eeprom_printf("EEPROM_WriteDataByte(0x%04x, 0x%02x) [SKIP SAME]\n", Address, DataByte);
+        return 0;
+    }
+
+    /* keep DataBuf cache in sync */
+    DataBuf[Address] = DataByte;
+    eeprom_printf("EEPROM_WriteDataByte DataBuf[0x%04x] = 0x%02x\n", Address, DataBuf[Address]);
+
+    /* perform the write into flash memory */
+    /* First, attempt to write directly into the compacted flash area */
+    FLASH_Status status = eeprom_write_direct_entry(Address);
+    if (!status) {
+        /* Otherwise append to the write log */
+        if (Address < FEE_BYTE_RANGE) {
+            status = eeprom_write_log_byte_entry(Address);
+        } else {
+            status = eeprom_write_log_word_entry(Address & 0xFFFE);
+        }
+    }
+    if (status != 0 && status != FLASH_COMPLETE) {
+        eeprom_printf("EEPROM_WriteDataByte [STATUS == %d]\n", status);
+    }
+    return status;
+}
+
+uint8_t EEPROM_WriteDataWord(uint16_t Address, uint16_t DataWord) {
+    /* if the address is out-of-bounds, do nothing */
+    if (Address >= FEE_DENSITY_BYTES) {
+        eeprom_printf("EEPROM_WriteDataWord(0x%04x, 0x%04x) [BAD ADDRESS]\n", Address, DataWord);
+        return FLASH_BAD_ADDRESS;
+    }
+
+    /* Check for word alignment */
+    FLASH_Status final_status = FLASH_COMPLETE;
+    if (Address % 2) {
+        final_status        = EEPROM_WriteDataByte(Address, DataWord);
+        FLASH_Status status = EEPROM_WriteDataByte(Address + 1, DataWord >> 8);
+        if (status != FLASH_COMPLETE) final_status = status;
+        if (final_status != 0 && final_status != FLASH_COMPLETE) {
+            eeprom_printf("EEPROM_WriteDataWord [STATUS == %d]\n", final_status);
+        }
+        return final_status;
+    }
+
+    /* if the value is the same, don't bother writing it */
+    uint16_t oldValue = *(uint16_t *)(&DataBuf[Address]);
+    if (oldValue == DataWord) {
+        eeprom_printf("EEPROM_WriteDataWord(0x%04x, 0x%04x) [SKIP SAME]\n", Address, DataWord);
+        return 0;
+    }
+
+    /* keep DataBuf cache in sync */
+    *(uint16_t *)(&DataBuf[Address]) = DataWord;
+    eeprom_printf("EEPROM_WriteDataWord DataBuf[0x%04x] = 0x%04x\n", Address, *(uint16_t *)(&DataBuf[Address]));
+
+    /* perform the write into flash memory */
+    /* First, attempt to write directly into the compacted flash area */
+    final_status = eeprom_write_direct_entry(Address);
+    if (!final_status) {
+        /* Otherwise append to the write log */
+        /* Check if we need to fall back to byte write */
+        if (Address < FEE_BYTE_RANGE) {
+            final_status = FLASH_COMPLETE;
+            /* Only write a byte if it has changed */
+            if ((uint8_t)oldValue != (uint8_t)DataWord) {
+                final_status = eeprom_write_log_byte_entry(Address);
+            }
+            FLASH_Status status = FLASH_COMPLETE;
+            /* Only write a byte if it has changed */
+            if ((oldValue >> 8) != (DataWord >> 8)) {
+                status = eeprom_write_log_byte_entry(Address + 1);
+            }
+            if (status != FLASH_COMPLETE) final_status = status;
+        } else {
+            final_status = eeprom_write_log_word_entry(Address);
+        }
+    }
+    if (final_status != 0 && final_status != FLASH_COMPLETE) {
+        eeprom_printf("EEPROM_WriteDataWord [STATUS == %d]\n", final_status);
+    }
+    return final_status;
+}
+
+uint8_t EEPROM_ReadDataByte(uint16_t Address) {
+    uint8_t DataByte = 0xFF;
+
+    if (Address < FEE_DENSITY_BYTES) {
+        DataByte = DataBuf[Address];
+    }
+
+    eeprom_printf("EEPROM_ReadDataByte(0x%04x): 0x%02x\n", Address, DataByte);
+
+    return DataByte;
+}
+
+uint16_t EEPROM_ReadDataWord(uint16_t Address) {
+    uint16_t DataWord = 0xFFFF;
+
+    if (Address < FEE_DENSITY_BYTES - 1) {
+        /* Check word alignment */
+        if (Address % 2) {
+            DataWord = DataBuf[Address] | (DataBuf[Address + 1] << 8);
+        } else {
+            DataWord = *(uint16_t *)(&DataBuf[Address]);
+        }
+    }
+
+    eeprom_printf("EEPROM_ReadDataWord(0x%04x): 0x%04x\n", Address, DataWord);
+
+    return DataWord;
+}
+
+/*****************************************************************************
+ *  Bind to eeprom_driver.c
+ *******************************************************************************/
+void eeprom_driver_init(void) {
+    EEPROM_Init();
+}
+
+void eeprom_driver_erase(void) {
+    EEPROM_Erase();
+}
+
+void eeprom_read_block(void *buf, const void *addr, size_t len) {
+    const uint8_t *src  = (const uint8_t *)addr;
+    uint8_t *      dest = (uint8_t *)buf;
+
+    /* Check word alignment */
+    if (len && (uintptr_t)src % 2) {
+        /* Read the unaligned first byte */
+        *dest++ = EEPROM_ReadDataByte((const uintptr_t)src++);
+        --len;
+    }
+
+    uint16_t value;
+    bool     aligned = ((uintptr_t)dest % 2 == 0);
+    while (len > 1) {
+        value = EEPROM_ReadDataWord((const uintptr_t)((uint16_t *)src));
+        if (aligned) {
+            *(uint16_t *)dest = value;
+            dest += 2;
+        } else {
+            *dest++ = value;
+            *dest++ = value >> 8;
+        }
+        src += 2;
+        len -= 2;
+    }
+    if (len) {
+        *dest = EEPROM_ReadDataByte((const uintptr_t)src);
+    }
+}
+
+void eeprom_write_block(const void *buf, void *addr, size_t len) {
+    uint8_t *      dest = (uint8_t *)addr;
+    const uint8_t *src  = (const uint8_t *)buf;
+
+    /* Check word alignment */
+    if (len && (uintptr_t)dest % 2) {
+        /* Write the unaligned first byte */
+        EEPROM_WriteDataByte((uintptr_t)dest++, *src++);
+        --len;
+    }
+
+    uint16_t value;
+    bool     aligned = ((uintptr_t)src % 2 == 0);
+    while (len > 1) {
+        if (aligned) {
+            value = *(uint16_t *)src;
+        } else {
+            value = *(uint8_t *)src | (*(uint8_t *)(src + 1) << 8);
+        }
+        EEPROM_WriteDataWord((uintptr_t)((uint16_t *)dest), value);
+        dest += 2;
+        src += 2;
+        len -= 2;
+    }
+
+    if (len) {
+        EEPROM_WriteDataByte((uintptr_t)dest, *src);
+    }
+}
diff --git a/platforms/chibios/drivers/eeprom/eeprom_stm32.h b/platforms/chibios/drivers/eeprom/eeprom_stm32.h
new file mode 100644
index 0000000000..8fcfb556b8
--- /dev/null
+++ b/platforms/chibios/drivers/eeprom/eeprom_stm32.h
@@ -0,0 +1,33 @@
+/*
+ * This software is experimental and a work in progress.
+ * Under no circumstances should these files be used in relation to any critical system(s).
+ * Use of these files is at your own risk.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+ * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * This files are free to use from http://engsta.com/stm32-flash-memory-eeprom-emulator/ by
+ * Artur F.
+ *
+ * Modifications for QMK and STM32F303 by Yiancar
+ *
+ * This library assumes 8-bit data locations. To add a new MCU, please provide the flash
+ * page size and the total flash size in Kb. The number of available pages must be a multiple
+ * of 2. Only half of the pages account for the total EEPROM size.
+ * This library also assumes that the pages are not used by the firmware.
+ */
+
+#pragma once
+
+uint16_t EEPROM_Init(void);
+void     EEPROM_Erase(void);
+uint8_t  EEPROM_WriteDataByte(uint16_t Address, uint8_t DataByte);
+uint8_t  EEPROM_WriteDataWord(uint16_t Address, uint16_t DataWord);
+uint8_t  EEPROM_ReadDataByte(uint16_t Address);
+uint16_t EEPROM_ReadDataWord(uint16_t Address);
+
+void print_eeprom(void);
diff --git a/platforms/chibios/drivers/eeprom/eeprom_stm32_defs.h b/platforms/chibios/drivers/eeprom/eeprom_stm32_defs.h
new file mode 100644
index 0000000000..57d0440330
--- /dev/null
+++ b/platforms/chibios/drivers/eeprom/eeprom_stm32_defs.h
@@ -0,0 +1,136 @@
+/* Copyright 2021 QMK
+ *
+ * This program 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 program 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/>.
+ */
+#pragma once
+
+#include <hal.h>
+
+#if !defined(FEE_PAGE_SIZE) || !defined(FEE_PAGE_COUNT)
+#    if defined(STM32F103xB) || defined(STM32F042x6) || defined(GD32VF103C8) || defined(GD32VF103CB)
+#        ifndef FEE_PAGE_SIZE
+#            define FEE_PAGE_SIZE 0x400 // Page size = 1KByte
+#        endif
+#        ifndef FEE_PAGE_COUNT
+#            define FEE_PAGE_COUNT 2 // How many pages are used
+#        endif
+#    elif defined(STM32F103xE) || defined(STM32F303xC) || defined(STM32F303xE) || defined(STM32F072xB) || defined(STM32F070xB)
+#        ifndef FEE_PAGE_SIZE
+#            define FEE_PAGE_SIZE 0x800 // Page size = 2KByte
+#        endif
+#        ifndef FEE_PAGE_COUNT
+#            define FEE_PAGE_COUNT 4 // How many pages are used
+#        endif
+#    elif defined(STM32F401xC) || defined(STM32F401xE) || defined(STM32F405xG) || defined(STM32F411xE)
+#        ifndef FEE_PAGE_SIZE
+#            define FEE_PAGE_SIZE 0x4000 // Page size = 16KByte
+#        endif
+#        ifndef FEE_PAGE_COUNT
+#            define FEE_PAGE_COUNT 1 // How many pages are used
+#        endif
+#    endif
+#endif
+
+#if !defined(FEE_MCU_FLASH_SIZE)
+#    if defined(STM32F042x6)
+#        define FEE_MCU_FLASH_SIZE 32 // Size in Kb
+#    elif defined(GD32VF103C8)
+#        define FEE_MCU_FLASH_SIZE 64 // Size in Kb
+#    elif defined(STM32F103xB) || defined(STM32F072xB) || defined(STM32F070xB) || defined(GD32VF103CB)
+#        define FEE_MCU_FLASH_SIZE 128 // Size in Kb
+#    elif defined(STM32F303xC) || defined(STM32F401xC)
+#        define FEE_MCU_FLASH_SIZE 256 // Size in Kb
+#    elif defined(STM32F103xE) || defined(STM32F303xE) || defined(STM32F401xE) || defined(STM32F411xE)
+#        define FEE_MCU_FLASH_SIZE 512 // Size in Kb
+#    elif defined(STM32F405xG)
+#        define FEE_MCU_FLASH_SIZE 1024 // Size in Kb
+#    endif
+#endif
+
+/* Start of the emulated eeprom */
+#if !defined(FEE_PAGE_BASE_ADDRESS)
+#    if defined(STM32F401xC) || defined(STM32F401xE) || defined(STM32F405xG) || defined(STM32F411xE)
+#        ifndef FEE_PAGE_BASE_ADDRESS
+#            define FEE_PAGE_BASE_ADDRESS 0x08004000 // bodge to force 2nd 16k page
+#        endif
+#    else
+#        ifndef FEE_FLASH_BASE
+#            define FEE_FLASH_BASE 0x8000000
+#        endif
+/* Default to end of flash */
+#        define FEE_PAGE_BASE_ADDRESS ((uintptr_t)(FEE_FLASH_BASE) + FEE_MCU_FLASH_SIZE * 1024 - (FEE_PAGE_COUNT * FEE_PAGE_SIZE))
+#    endif
+#endif
+
+/* Addressable range 16KByte: 0 <-> (0x1FFF << 1) */
+#define FEE_ADDRESS_MAX_SIZE 0x4000
+
+/* Size of combined compacted eeprom and write log pages */
+#define FEE_DENSITY_MAX_SIZE (FEE_PAGE_COUNT * FEE_PAGE_SIZE)
+
+#ifndef FEE_MCU_FLASH_SIZE_IGNORE_CHECK /* *TODO: Get rid of this check */
+#    if FEE_DENSITY_MAX_SIZE > (FEE_MCU_FLASH_SIZE * 1024)
+#        pragma message STR(FEE_DENSITY_MAX_SIZE) " > " STR(FEE_MCU_FLASH_SIZE * 1024)
+#        error emulated eeprom: FEE_DENSITY_MAX_SIZE is greater than available flash size
+#    endif
+#endif
+
+/* Size of emulated eeprom */
+#ifdef FEE_DENSITY_BYTES
+#    if (FEE_DENSITY_BYTES > FEE_DENSITY_MAX_SIZE)
+#        pragma message STR(FEE_DENSITY_BYTES) " > " STR(FEE_DENSITY_MAX_SIZE)
+#        error emulated eeprom: FEE_DENSITY_BYTES exceeds FEE_DENSITY_MAX_SIZE
+#    endif
+#    if (FEE_DENSITY_BYTES == FEE_DENSITY_MAX_SIZE)
+#        pragma message STR(FEE_DENSITY_BYTES) " == " STR(FEE_DENSITY_MAX_SIZE)
+#        warning emulated eeprom: FEE_DENSITY_BYTES leaves no room for a write log.  This will greatly increase the flash wear rate!
+#    endif
+#    if FEE_DENSITY_BYTES > FEE_ADDRESS_MAX_SIZE
+#        pragma message STR(FEE_DENSITY_BYTES) " > " STR(FEE_ADDRESS_MAX_SIZE)
+#        error emulated eeprom: FEE_DENSITY_BYTES is greater than FEE_ADDRESS_MAX_SIZE allows
+#    endif
+#    if ((FEE_DENSITY_BYTES) % 2) == 1
+#        error emulated eeprom: FEE_DENSITY_BYTES must be even
+#    endif
+#else
+/* Default to half of allocated space used for emulated eeprom, half for write log */
+#    define FEE_DENSITY_BYTES (FEE_PAGE_COUNT * FEE_PAGE_SIZE / 2)
+#endif
+
+/* Size of write log */
+#ifdef FEE_WRITE_LOG_BYTES
+#    if ((FEE_DENSITY_BYTES + FEE_WRITE_LOG_BYTES) > FEE_DENSITY_MAX_SIZE)
+#        pragma message STR(FEE_DENSITY_BYTES) " + " STR(FEE_WRITE_LOG_BYTES) " > " STR(FEE_DENSITY_MAX_SIZE)
+#        error emulated eeprom: FEE_WRITE_LOG_BYTES exceeds remaining FEE_DENSITY_MAX_SIZE
+#    endif
+#    if ((FEE_WRITE_LOG_BYTES) % 2) == 1
+#        error emulated eeprom: FEE_WRITE_LOG_BYTES must be even
+#    endif
+#else
+/* Default to use all remaining space */
+#    define FEE_WRITE_LOG_BYTES (FEE_PAGE_COUNT * FEE_PAGE_SIZE - FEE_DENSITY_BYTES)
+#endif
+
+/* Start of the emulated eeprom compacted flash area */
+#define FEE_COMPACTED_BASE_ADDRESS FEE_PAGE_BASE_ADDRESS
+/* End of the emulated eeprom compacted flash area */
+#define FEE_COMPACTED_LAST_ADDRESS (FEE_COMPACTED_BASE_ADDRESS + FEE_DENSITY_BYTES)
+/* Start of the emulated eeprom write log */
+#define FEE_WRITE_LOG_BASE_ADDRESS FEE_COMPACTED_LAST_ADDRESS
+/* End of the emulated eeprom write log */
+#define FEE_WRITE_LOG_LAST_ADDRESS (FEE_WRITE_LOG_BASE_ADDRESS + FEE_WRITE_LOG_BYTES)
+
+#if defined(DYNAMIC_KEYMAP_EEPROM_MAX_ADDR) && (DYNAMIC_KEYMAP_EEPROM_MAX_ADDR >= FEE_DENSITY_BYTES)
+#    error emulated eeprom: DYNAMIC_KEYMAP_EEPROM_MAX_ADDR is greater than the FEE_DENSITY_BYTES available
+#endif
diff --git a/platforms/chibios/drivers/eeprom/eeprom_teensy.c b/platforms/chibios/drivers/eeprom/eeprom_teensy.c
new file mode 100644
index 0000000000..c8777febde
--- /dev/null
+++ b/platforms/chibios/drivers/eeprom/eeprom_teensy.c
@@ -0,0 +1,546 @@
+#include <ch.h>
+#include <hal.h>
+
+#include "eeprom_teensy.h"
+#include "eeconfig.h"
+
+/*************************************/
+/*          Hardware backend         */
+/*                                   */
+/*    Code from PJRC/Teensyduino     */
+/*************************************/
+
+/* Teensyduino Core Library
+ * http://www.pjrc.com/teensy/
+ * Copyright (c) 2013 PJRC.COM, LLC.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * 1. The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * 2. If the Software is incorporated into a build system that allows
+ * selection among a list of target devices, then similar target
+ * devices manufactured by PJRC.COM must be included in the list of
+ * target devices and selectable in the same manner.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#if defined(K20x) /* chip selection */
+/* Teensy 3.0, 3.1, 3.2; mchck; infinity keyboard */
+
+/*
+    ^^^ Here be dragons:
+        NXP AppNote AN4282 section 3.1 states that partitioning must only be done once.
+        Once EEPROM partitioning is done, the size is locked to this initial configuration.
+        Attempts to modify the EEPROM_SIZE setting may brick your board.
+*/
+
+// Writing unaligned 16 or 32 bit data is handled automatically when
+// this is defined, but at a cost of extra code size.  Without this,
+// any unaligned write will cause a hard fault exception!  If you're
+// absolutely sure all 16 and 32 bit writes will be aligned, you can
+// remove the extra unnecessary code.
+//
+#    define HANDLE_UNALIGNED_WRITES
+
+// Minimum EEPROM Endurance
+// ------------------------
+#    if (EEPROM_SIZE == 2048) // 35000 writes/byte or 70000 writes/word
+#        define EEESIZE 0x33
+#    elif (EEPROM_SIZE == 1024) // 75000 writes/byte or 150000 writes/word
+#        define EEESIZE 0x34
+#    elif (EEPROM_SIZE == 512) // 155000 writes/byte or 310000 writes/word
+#        define EEESIZE 0x35
+#    elif (EEPROM_SIZE == 256) // 315000 writes/byte or 630000 writes/word
+#        define EEESIZE 0x36
+#    elif (EEPROM_SIZE == 128) // 635000 writes/byte or 1270000 writes/word
+#        define EEESIZE 0x37
+#    elif (EEPROM_SIZE == 64) // 1275000 writes/byte or 2550000 writes/word
+#        define EEESIZE 0x38
+#    elif (EEPROM_SIZE == 32) // 2555000 writes/byte or 5110000 writes/word
+#        define EEESIZE 0x39
+#    endif
+
+/** \brief eeprom initialization
+ *
+ * FIXME: needs doc
+ */
+void eeprom_initialize(void) {
+    uint32_t count          = 0;
+    uint16_t do_flash_cmd[] = {0xf06f, 0x037f, 0x7003, 0x7803, 0xf013, 0x0f80, 0xd0fb, 0x4770};
+    uint8_t  status;
+
+    if (FTFL->FCNFG & FTFL_FCNFG_RAMRDY) {
+        // FlexRAM is configured as traditional RAM
+        // We need to reconfigure for EEPROM usage
+        FTFL->FCCOB0 = 0x80;    // PGMPART = Program Partition Command
+        FTFL->FCCOB4 = EEESIZE; // EEPROM Size
+        FTFL->FCCOB5 = 0x03;    // 0K for Dataflash, 32K for EEPROM backup
+        __disable_irq();
+        // do_flash_cmd() must execute from RAM.  Luckily the C syntax is simple...
+        (*((void (*)(volatile uint8_t *))((uint32_t)do_flash_cmd | 1)))(&(FTFL->FSTAT));
+        __enable_irq();
+        status = FTFL->FSTAT;
+        if (status & (FTFL_FSTAT_RDCOLERR | FTFL_FSTAT_ACCERR | FTFL_FSTAT_FPVIOL)) {
+            FTFL->FSTAT = (status & (FTFL_FSTAT_RDCOLERR | FTFL_FSTAT_ACCERR | FTFL_FSTAT_FPVIOL));
+            return; // error
+        }
+    }
+    // wait for eeprom to become ready (is this really necessary?)
+    while (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) {
+        if (++count > 20000) break;
+    }
+}
+
+#    define FlexRAM ((uint8_t *)0x14000000)
+
+/** \brief eeprom read byte
+ *
+ * FIXME: needs doc
+ */
+uint8_t eeprom_read_byte(const uint8_t *addr) {
+    uint32_t offset = (uint32_t)addr;
+    if (offset >= EEPROM_SIZE) return 0;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+    return FlexRAM[offset];
+}
+
+/** \brief eeprom read word
+ *
+ * FIXME: needs doc
+ */
+uint16_t eeprom_read_word(const uint16_t *addr) {
+    uint32_t offset = (uint32_t)addr;
+    if (offset >= EEPROM_SIZE - 1) return 0;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+    return *(uint16_t *)(&FlexRAM[offset]);
+}
+
+/** \brief eeprom read dword
+ *
+ * FIXME: needs doc
+ */
+uint32_t eeprom_read_dword(const uint32_t *addr) {
+    uint32_t offset = (uint32_t)addr;
+    if (offset >= EEPROM_SIZE - 3) return 0;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+    return *(uint32_t *)(&FlexRAM[offset]);
+}
+
+/** \brief eeprom read block
+ *
+ * FIXME: needs doc
+ */
+void eeprom_read_block(void *buf, const void *addr, uint32_t len) {
+    uint32_t offset = (uint32_t)addr;
+    uint8_t *dest   = (uint8_t *)buf;
+    uint32_t end    = offset + len;
+
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+    if (end > EEPROM_SIZE) end = EEPROM_SIZE;
+    while (offset < end) {
+        *dest++ = FlexRAM[offset++];
+    }
+}
+
+/** \brief eeprom is ready
+ *
+ * FIXME: needs doc
+ */
+int eeprom_is_ready(void) {
+    return (FTFL->FCNFG & FTFL_FCNFG_EEERDY) ? 1 : 0;
+}
+
+/** \brief flexram wait
+ *
+ * FIXME: needs doc
+ */
+static void flexram_wait(void) {
+    while (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) {
+        // TODO: timeout
+    }
+}
+
+/** \brief eeprom_write_byte
+ *
+ * FIXME: needs doc
+ */
+void eeprom_write_byte(uint8_t *addr, uint8_t value) {
+    uint32_t offset = (uint32_t)addr;
+
+    if (offset >= EEPROM_SIZE) return;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+    if (FlexRAM[offset] != value) {
+        FlexRAM[offset] = value;
+        flexram_wait();
+    }
+}
+
+/** \brief eeprom write word
+ *
+ * FIXME: needs doc
+ */
+void eeprom_write_word(uint16_t *addr, uint16_t value) {
+    uint32_t offset = (uint32_t)addr;
+
+    if (offset >= EEPROM_SIZE - 1) return;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+#    ifdef HANDLE_UNALIGNED_WRITES
+    if ((offset & 1) == 0) {
+#    endif
+        if (*(uint16_t *)(&FlexRAM[offset]) != value) {
+            *(uint16_t *)(&FlexRAM[offset]) = value;
+            flexram_wait();
+        }
+#    ifdef HANDLE_UNALIGNED_WRITES
+    } else {
+        if (FlexRAM[offset] != value) {
+            FlexRAM[offset] = value;
+            flexram_wait();
+        }
+        if (FlexRAM[offset + 1] != (value >> 8)) {
+            FlexRAM[offset + 1] = value >> 8;
+            flexram_wait();
+        }
+    }
+#    endif
+}
+
+/** \brief eeprom write dword
+ *
+ * FIXME: needs doc
+ */
+void eeprom_write_dword(uint32_t *addr, uint32_t value) {
+    uint32_t offset = (uint32_t)addr;
+
+    if (offset >= EEPROM_SIZE - 3) return;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+#    ifdef HANDLE_UNALIGNED_WRITES
+    switch (offset & 3) {
+        case 0:
+#    endif
+            if (*(uint32_t *)(&FlexRAM[offset]) != value) {
+                *(uint32_t *)(&FlexRAM[offset]) = value;
+                flexram_wait();
+            }
+            return;
+#    ifdef HANDLE_UNALIGNED_WRITES
+        case 2:
+            if (*(uint16_t *)(&FlexRAM[offset]) != value) {
+                *(uint16_t *)(&FlexRAM[offset]) = value;
+                flexram_wait();
+            }
+            if (*(uint16_t *)(&FlexRAM[offset + 2]) != (value >> 16)) {
+                *(uint16_t *)(&FlexRAM[offset + 2]) = value >> 16;
+                flexram_wait();
+            }
+            return;
+        default:
+            if (FlexRAM[offset] != value) {
+                FlexRAM[offset] = value;
+                flexram_wait();
+            }
+            if (*(uint16_t *)(&FlexRAM[offset + 1]) != (value >> 8)) {
+                *(uint16_t *)(&FlexRAM[offset + 1]) = value >> 8;
+                flexram_wait();
+            }
+            if (FlexRAM[offset + 3] != (value >> 24)) {
+                FlexRAM[offset + 3] = value >> 24;
+                flexram_wait();
+            }
+    }
+#    endif
+}
+
+/** \brief eeprom write block
+ *
+ * FIXME: needs doc
+ */
+void eeprom_write_block(const void *buf, void *addr, uint32_t len) {
+    uint32_t       offset = (uint32_t)addr;
+    const uint8_t *src    = (const uint8_t *)buf;
+
+    if (offset >= EEPROM_SIZE) return;
+    if (!(FTFL->FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
+    if (len >= EEPROM_SIZE) len = EEPROM_SIZE;
+    if (offset + len >= EEPROM_SIZE) len = EEPROM_SIZE - offset;
+    while (len > 0) {
+        uint32_t lsb = offset & 3;
+        if (lsb == 0 && len >= 4) {
+            // write aligned 32 bits
+            uint32_t val32;
+            val32 = *src++;
+            val32 |= (*src++ << 8);
+            val32 |= (*src++ << 16);
+            val32 |= (*src++ << 24);
+            if (*(uint32_t *)(&FlexRAM[offset]) != val32) {
+                *(uint32_t *)(&FlexRAM[offset]) = val32;
+                flexram_wait();
+            }
+            offset += 4;
+            len -= 4;
+        } else if ((lsb == 0 || lsb == 2) && len >= 2) {
+            // write aligned 16 bits
+            uint16_t val16;
+            val16 = *src++;
+            val16 |= (*src++ << 8);
+            if (*(uint16_t *)(&FlexRAM[offset]) != val16) {
+                *(uint16_t *)(&FlexRAM[offset]) = val16;
+                flexram_wait();
+            }
+            offset += 2;
+            len -= 2;
+        } else {
+            // write 8 bits
+            uint8_t val8 = *src++;
+            if (FlexRAM[offset] != val8) {
+                FlexRAM[offset] = val8;
+                flexram_wait();
+            }
+            offset++;
+            len--;
+        }
+    }
+}
+
+/*
+void do_flash_cmd(volatile uint8_t *fstat)
+{
+    *fstat = 0x80;
+    while ((*fstat & 0x80) == 0) ; // wait
+}
+00000000 <do_flash_cmd>:
+   0:	f06f 037f 	mvn.w	r3, #127	; 0x7f
+   4:	7003      	strb	r3, [r0, #0]
+   6:	7803      	ldrb	r3, [r0, #0]
+   8:	f013 0f80 	tst.w	r3, #128	; 0x80
+   c:	d0fb      	beq.n	6 <do_flash_cmd+0x6>
+   e:	4770      	bx	lr
+*/
+
+#elif defined(KL2x) /* chip selection */
+/* Teensy LC (emulated) */
+
+#    define SYMVAL(sym) (uint32_t)(((uint8_t *)&(sym)) - ((uint8_t *)0))
+
+extern uint32_t __eeprom_workarea_start__;
+extern uint32_t __eeprom_workarea_end__;
+
+static uint32_t flashend = 0;
+
+void eeprom_initialize(void) {
+    const uint16_t *p = (uint16_t *)SYMVAL(__eeprom_workarea_start__);
+
+    do {
+        if (*p++ == 0xFFFF) {
+            flashend = (uint32_t)(p - 2);
+            return;
+        }
+    } while (p < (uint16_t *)SYMVAL(__eeprom_workarea_end__));
+    flashend = (uint32_t)(p - 1);
+}
+
+uint8_t eeprom_read_byte(const uint8_t *addr) {
+    uint32_t        offset = (uint32_t)addr;
+    const uint16_t *p      = (uint16_t *)SYMVAL(__eeprom_workarea_start__);
+    const uint16_t *end    = (const uint16_t *)((uint32_t)flashend);
+    uint16_t        val;
+    uint8_t         data = 0xFF;
+
+    if (!end) {
+        eeprom_initialize();
+        end = (const uint16_t *)((uint32_t)flashend);
+    }
+    if (offset < EEPROM_SIZE) {
+        while (p <= end) {
+            val = *p++;
+            if ((val & 255) == offset) data = val >> 8;
+        }
+    }
+    return data;
+}
+
+static void flash_write(const uint16_t *code, uint32_t addr, uint32_t data) {
+    // with great power comes great responsibility....
+    uint32_t stat;
+    *(uint32_t *)&(FTFA->FCCOB3) = 0x06000000 | (addr & 0x00FFFFFC);
+    *(uint32_t *)&(FTFA->FCCOB7) = data;
+    __disable_irq();
+    (*((void (*)(volatile uint8_t *))((uint32_t)code | 1)))(&(FTFA->FSTAT));
+    __enable_irq();
+    stat = FTFA->FSTAT & (FTFA_FSTAT_RDCOLERR | FTFA_FSTAT_ACCERR | FTFA_FSTAT_FPVIOL);
+    if (stat) {
+        FTFA->FSTAT = stat;
+    }
+    MCM->PLACR |= MCM_PLACR_CFCC;
+}
+
+void eeprom_write_byte(uint8_t *addr, uint8_t data) {
+    uint32_t        offset = (uint32_t)addr;
+    const uint16_t *p, *end = (const uint16_t *)((uint32_t)flashend);
+    uint32_t        i, val, flashaddr;
+    uint16_t        do_flash_cmd[] = {0x2380, 0x7003, 0x7803, 0xb25b, 0x2b00, 0xdafb, 0x4770};
+    uint8_t         buf[EEPROM_SIZE];
+
+    if (offset >= EEPROM_SIZE) return;
+    if (!end) {
+        eeprom_initialize();
+        end = (const uint16_t *)((uint32_t)flashend);
+    }
+    if (++end < (uint16_t *)SYMVAL(__eeprom_workarea_end__)) {
+        val       = (data << 8) | offset;
+        flashaddr = (uint32_t)end;
+        flashend  = flashaddr;
+        if ((flashaddr & 2) == 0) {
+            val |= 0xFFFF0000;
+        } else {
+            val <<= 16;
+            val |= 0x0000FFFF;
+        }
+        flash_write(do_flash_cmd, flashaddr, val);
+    } else {
+        for (i = 0; i < EEPROM_SIZE; i++) {
+            buf[i] = 0xFF;
+        }
+        val = 0;
+        for (p = (uint16_t *)SYMVAL(__eeprom_workarea_start__); p < (uint16_t *)SYMVAL(__eeprom_workarea_end__); p++) {
+            val = *p;
+            if ((val & 255) < EEPROM_SIZE) {
+                buf[val & 255] = val >> 8;
+            }
+        }
+        buf[offset] = data;
+        for (flashaddr = (uint32_t)(uint16_t *)SYMVAL(__eeprom_workarea_start__); flashaddr < (uint32_t)(uint16_t *)SYMVAL(__eeprom_workarea_end__); flashaddr += 1024) {
+            *(uint32_t *)&(FTFA->FCCOB3) = 0x09000000 | flashaddr;
+            __disable_irq();
+            (*((void (*)(volatile uint8_t *))((uint32_t)do_flash_cmd | 1)))(&(FTFA->FSTAT));
+            __enable_irq();
+            val = FTFA->FSTAT & (FTFA_FSTAT_RDCOLERR | FTFA_FSTAT_ACCERR | FTFA_FSTAT_FPVIOL);
+            ;
+            if (val) FTFA->FSTAT = val;
+            MCM->PLACR |= MCM_PLACR_CFCC;
+        }
+        flashaddr = (uint32_t)(uint16_t *)SYMVAL(__eeprom_workarea_start__);
+        for (i = 0; i < EEPROM_SIZE; i++) {
+            if (buf[i] == 0xFF) continue;
+            if ((flashaddr & 2) == 0) {
+                val = (buf[i] << 8) | i;
+            } else {
+                val = val | (buf[i] << 24) | (i << 16);
+                flash_write(do_flash_cmd, flashaddr, val);
+            }
+            flashaddr += 2;
+        }
+        flashend = flashaddr;
+        if ((flashaddr & 2)) {
+            val |= 0xFFFF0000;
+            flash_write(do_flash_cmd, flashaddr, val);
+        }
+    }
+}
+
+/*
+void do_flash_cmd(volatile uint8_t *fstat)
+{
+        *fstat = 0x80;
+        while ((*fstat & 0x80) == 0) ; // wait
+}
+00000000 <do_flash_cmd>:
+   0:	2380      	movs	r3, #128	; 0x80
+   2:	7003      	strb	r3, [r0, #0]
+   4:	7803      	ldrb	r3, [r0, #0]
+   6:	b25b      	sxtb	r3, r3
+   8:	2b00      	cmp	r3, #0
+   a:	dafb      	bge.n	4 <do_flash_cmd+0x4>
+   c:	4770      	bx	lr
+*/
+
+uint16_t eeprom_read_word(const uint16_t *addr) {
+    const uint8_t *p = (const uint8_t *)addr;
+    return eeprom_read_byte(p) | (eeprom_read_byte(p + 1) << 8);
+}
+
+uint32_t eeprom_read_dword(const uint32_t *addr) {
+    const uint8_t *p = (const uint8_t *)addr;
+    return eeprom_read_byte(p) | (eeprom_read_byte(p + 1) << 8) | (eeprom_read_byte(p + 2) << 16) | (eeprom_read_byte(p + 3) << 24);
+}
+
+void eeprom_read_block(void *buf, const void *addr, uint32_t len) {
+    const uint8_t *p    = (const uint8_t *)addr;
+    uint8_t *      dest = (uint8_t *)buf;
+    while (len--) {
+        *dest++ = eeprom_read_byte(p++);
+    }
+}
+
+int eeprom_is_ready(void) {
+    return 1;
+}
+
+void eeprom_write_word(uint16_t *addr, uint16_t value) {
+    uint8_t *p = (uint8_t *)addr;
+    eeprom_write_byte(p++, value);
+    eeprom_write_byte(p, value >> 8);
+}
+
+void eeprom_write_dword(uint32_t *addr, uint32_t value) {
+    uint8_t *p = (uint8_t *)addr;
+    eeprom_write_byte(p++, value);
+    eeprom_write_byte(p++, value >> 8);
+    eeprom_write_byte(p++, value >> 16);
+    eeprom_write_byte(p, value >> 24);
+}
+
+void eeprom_write_block(const void *buf, void *addr, uint32_t len) {
+    uint8_t *      p   = (uint8_t *)addr;
+    const uint8_t *src = (const uint8_t *)buf;
+    while (len--) {
+        eeprom_write_byte(p++, *src++);
+    }
+}
+
+#else
+#    error Unsupported Teensy EEPROM.
+#endif /* chip selection */
+// The update functions just calls write for now, but could probably be optimized
+
+void eeprom_update_byte(uint8_t *addr, uint8_t value) {
+    eeprom_write_byte(addr, value);
+}
+
+void eeprom_update_word(uint16_t *addr, uint16_t value) {
+    uint8_t *p = (uint8_t *)addr;
+    eeprom_write_byte(p++, value);
+    eeprom_write_byte(p, value >> 8);
+}
+
+void eeprom_update_dword(uint32_t *addr, uint32_t value) {
+    uint8_t *p = (uint8_t *)addr;
+    eeprom_write_byte(p++, value);
+    eeprom_write_byte(p++, value >> 8);
+    eeprom_write_byte(p++, value >> 16);
+    eeprom_write_byte(p, value >> 24);
+}
+
+void eeprom_update_block(const void *buf, void *addr, size_t len) {
+    uint8_t *      p   = (uint8_t *)addr;
+    const uint8_t *src = (const uint8_t *)buf;
+    while (len--) {
+        eeprom_write_byte(p++, *src++);
+    }
+}
diff --git a/platforms/chibios/drivers/eeprom/eeprom_teensy.h b/platforms/chibios/drivers/eeprom/eeprom_teensy.h
new file mode 100755
index 0000000000..9a14a1fa79
--- /dev/null
+++ b/platforms/chibios/drivers/eeprom/eeprom_teensy.h
@@ -0,0 +1,25 @@
+// Copyright 2022 Nick Brassel (@tzarc)
+// SPDX-License-Identifier: GPL-2.0-or-later
+#pragma once
+
+#include <ch.h>
+#include <hal.h>
+
+#if defined(K20x)
+/* Teensy 3.0, 3.1, 3.2; mchck; infinity keyboard */
+// The EEPROM is really RAM with a hardware-based backup system to
+// flash memory.  Selecting a smaller size EEPROM allows more wear
+// leveling, for higher write endurance.  If you edit this file,
+// set this to the smallest size your application can use.  Also,
+// due to Freescale's implementation, writing 16 or 32 bit words
+// (aligned to 2 or 4 byte boundaries) has twice the endurance
+// compared to writing 8 bit bytes.
+//
+#    ifndef EEPROM_SIZE
+#        define EEPROM_SIZE 32
+#    endif
+#elif defined(KL2x) /* Teensy LC (emulated) */
+#    define EEPROM_SIZE 128
+#else
+#    error Unsupported Teensy EEPROM.
+#endif
diff --git a/platforms/chibios/drivers/flash/flash_stm32.c b/platforms/chibios/drivers/flash/flash_stm32.c
new file mode 100644
index 0000000000..72c41b8b78
--- /dev/null
+++ b/platforms/chibios/drivers/flash/flash_stm32.c
@@ -0,0 +1,208 @@
+/*
+ * This software is experimental and a work in progress.
+ * Under no circumstances should these files be used in relation to any critical system(s).
+ * Use of these files is at your own risk.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+ * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * This files are free to use from https://github.com/rogerclarkmelbourne/Arduino_STM32 and
+ * https://github.com/leaflabs/libmaple
+ *
+ * Modifications for QMK and STM32F303 by Yiancar
+ */
+
+#include <hal.h>
+#include "flash_stm32.h"
+
+#if defined(STM32F1XX)
+#    define FLASH_SR_WRPERR FLASH_SR_WRPRTERR
+#endif
+
+#if defined(MCU_GD32V)
+/* GigaDevice GD32VF103 is a STM32F103 clone at heart. */
+#    include "gd32v_compatibility.h"
+#endif
+
+#if defined(STM32F4XX)
+#    define FLASH_SR_PGERR (FLASH_SR_PGSERR | FLASH_SR_PGPERR | FLASH_SR_PGAERR)
+
+#    define FLASH_KEY1 0x45670123U
+#    define FLASH_KEY2 0xCDEF89ABU
+
+static uint8_t ADDR2PAGE(uint32_t Page_Address) {
+    switch (Page_Address) {
+        case 0x08000000 ... 0x08003FFF:
+            return 0;
+        case 0x08004000 ... 0x08007FFF:
+            return 1;
+        case 0x08008000 ... 0x0800BFFF:
+            return 2;
+        case 0x0800C000 ... 0x0800FFFF:
+            return 3;
+    }
+
+    // TODO: bad times...
+    return 7;
+}
+#endif
+
+/* Delay definition */
+#define EraseTimeout ((uint32_t)0x00000FFF)
+#define ProgramTimeout ((uint32_t)0x0000001F)
+
+#define ASSERT(exp) (void)((0))
+
+/**
+ * @brief  Inserts a time delay.
+ * @param  None
+ * @retval None
+ */
+static void delay(void) {
+    __IO uint32_t i = 0;
+    for (i = 0xFF; i != 0; i--) {
+    }
+}
+
+/**
+ * @brief  Returns the FLASH Status.
+ * @param  None
+ * @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PG,
+ *   FLASH_ERROR_WRP or FLASH_COMPLETE
+ */
+FLASH_Status FLASH_GetStatus(void) {
+    if ((FLASH->SR & FLASH_SR_BSY) == FLASH_SR_BSY) return FLASH_BUSY;
+
+    if ((FLASH->SR & FLASH_SR_PGERR) != 0) return FLASH_ERROR_PG;
+
+    if ((FLASH->SR & FLASH_SR_WRPERR) != 0) return FLASH_ERROR_WRP;
+
+#if defined(FLASH_OBR_OPTERR)
+    if ((FLASH->SR & FLASH_OBR_OPTERR) != 0) return FLASH_ERROR_OPT;
+#endif
+
+    return FLASH_COMPLETE;
+}
+
+/**
+ * @brief  Waits for a Flash operation to complete or a TIMEOUT to occur.
+ * @param  Timeout: FLASH progamming Timeout
+ * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
+ *   FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
+ */
+FLASH_Status FLASH_WaitForLastOperation(uint32_t Timeout) {
+    FLASH_Status status;
+
+    /* Check for the Flash Status */
+    status = FLASH_GetStatus();
+    /* Wait for a Flash operation to complete or a TIMEOUT to occur */
+    while ((status == FLASH_BUSY) && (Timeout != 0x00)) {
+        delay();
+        status = FLASH_GetStatus();
+        Timeout--;
+    }
+    if (Timeout == 0) status = FLASH_TIMEOUT;
+    /* Return the operation status */
+    return status;
+}
+
+/**
+ * @brief  Erases a specified FLASH page.
+ * @param  Page_Address: The page address to be erased.
+ * @retval FLASH Status: The returned value can be: FLASH_BUSY, FLASH_ERROR_PG,
+ *   FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
+ */
+FLASH_Status FLASH_ErasePage(uint32_t Page_Address) {
+    FLASH_Status status = FLASH_COMPLETE;
+    /* Check the parameters */
+    ASSERT(IS_FLASH_ADDRESS(Page_Address));
+    /* Wait for last operation to be completed */
+    status = FLASH_WaitForLastOperation(EraseTimeout);
+
+    if (status == FLASH_COMPLETE) {
+        /* if the previous operation is completed, proceed to erase the page */
+#if defined(FLASH_CR_SNB)
+        FLASH->CR &= ~FLASH_CR_SNB;
+        FLASH->CR |= FLASH_CR_SER | (ADDR2PAGE(Page_Address) << FLASH_CR_SNB_Pos);
+#else
+        FLASH->CR |= FLASH_CR_PER;
+        FLASH->AR = Page_Address;
+#endif
+        FLASH->CR |= FLASH_CR_STRT;
+
+        /* Wait for last operation to be completed */
+        status = FLASH_WaitForLastOperation(EraseTimeout);
+        if (status != FLASH_TIMEOUT) {
+            /* if the erase operation is completed, disable the configured Bits */
+#if defined(FLASH_CR_SNB)
+            FLASH->CR &= ~(FLASH_CR_SER | FLASH_CR_SNB);
+#else
+            FLASH->CR &= ~FLASH_CR_PER;
+#endif
+        }
+        FLASH->SR = (FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPERR);
+    }
+    /* Return the Erase Status */
+    return status;
+}
+
+/**
+ * @brief  Programs a half word at a specified address.
+ * @param  Address: specifies the address to be programmed.
+ * @param  Data: specifies the data to be programmed.
+ * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
+ *   FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
+ */
+FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data) {
+    FLASH_Status status = FLASH_BAD_ADDRESS;
+
+    if (IS_FLASH_ADDRESS(Address)) {
+        /* Wait for last operation to be completed */
+        status = FLASH_WaitForLastOperation(ProgramTimeout);
+        if (status == FLASH_COMPLETE) {
+            /* if the previous operation is completed, proceed to program the new data */
+
+#if defined(FLASH_CR_PSIZE)
+            FLASH->CR &= ~FLASH_CR_PSIZE;
+            FLASH->CR |= FLASH_CR_PSIZE_0;
+#endif
+            FLASH->CR |= FLASH_CR_PG;
+            *(__IO uint16_t*)Address = Data;
+            /* Wait for last operation to be completed */
+            status = FLASH_WaitForLastOperation(ProgramTimeout);
+            if (status != FLASH_TIMEOUT) {
+                /* if the program operation is completed, disable the PG Bit */
+                FLASH->CR &= ~FLASH_CR_PG;
+            }
+            FLASH->SR = (FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPERR);
+        }
+    }
+    return status;
+}
+
+/**
+ * @brief  Unlocks the FLASH Program Erase Controller.
+ * @param  None
+ * @retval None
+ */
+void FLASH_Unlock(void) {
+    if (FLASH->CR & FLASH_CR_LOCK) {
+        /* Authorize the FPEC Access */
+        FLASH->KEYR = FLASH_KEY1;
+        FLASH->KEYR = FLASH_KEY2;
+    }
+}
+
+/**
+ * @brief  Locks the FLASH Program Erase Controller.
+ * @param  None
+ * @retval None
+ */
+void FLASH_Lock(void) {
+    /* Set the Lock Bit to lock the FPEC and the FCR */
+    FLASH->CR |= FLASH_CR_LOCK;
+}
diff --git a/platforms/chibios/drivers/flash/flash_stm32.h b/platforms/chibios/drivers/flash/flash_stm32.h
new file mode 100644
index 0000000000..6c66642ec5
--- /dev/null
+++ b/platforms/chibios/drivers/flash/flash_stm32.h
@@ -0,0 +1,44 @@
+/*
+ * This software is experimental and a work in progress.
+ * Under no circumstances should these files be used in relation to any critical system(s).
+ * Use of these files is at your own risk.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+ * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * This files are free to use from https://github.com/rogerclarkmelbourne/Arduino_STM32 and
+ * https://github.com/leaflabs/libmaple
+ *
+ * Modifications for QMK and STM32F303 by Yiancar
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#ifdef FLASH_STM32_MOCKED
+extern uint8_t FlashBuf[MOCK_FLASH_SIZE];
+#endif
+
+typedef enum { FLASH_BUSY = 1, FLASH_ERROR_PG, FLASH_ERROR_WRP, FLASH_ERROR_OPT, FLASH_COMPLETE, FLASH_TIMEOUT, FLASH_BAD_ADDRESS } FLASH_Status;
+
+#define IS_FLASH_ADDRESS(ADDRESS) (((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x0807FFFF))
+
+FLASH_Status FLASH_WaitForLastOperation(uint32_t Timeout);
+FLASH_Status FLASH_ErasePage(uint32_t Page_Address);
+FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data);
+
+void FLASH_Unlock(void);
+void FLASH_Lock(void);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/platforms/chibios/drivers/i2c_master.c b/platforms/chibios/drivers/i2c_master.c
index 21e064b1dc..4c7a5daa17 100644
--- a/platforms/chibios/drivers/i2c_master.c
+++ b/platforms/chibios/drivers/i2c_master.c
@@ -107,16 +107,25 @@ static const I2CConfig i2cconfig = {
 #endif
 };
 
-static i2c_status_t chibios_to_qmk(const msg_t* status) {
-    switch (*status) {
-        case I2C_NO_ERROR:
-            return I2C_STATUS_SUCCESS;
-        case I2C_TIMEOUT:
-            return I2C_STATUS_TIMEOUT;
-        // I2C_BUS_ERROR, I2C_ARBITRATION_LOST, I2C_ACK_FAILURE, I2C_OVERRUN, I2C_PEC_ERROR, I2C_SMB_ALERT
-        default:
-            return I2C_STATUS_ERROR;
+/**
+ * @brief Handles any I2C error condition by stopping the I2C peripheral and
+ * aborting any ongoing transactions. Furthermore ChibiOS status codes are
+ * converted into QMK codes.
+ *
+ * @param status ChibiOS specific I2C status code
+ * @return i2c_status_t QMK specific I2C status code
+ */
+static i2c_status_t i2c_epilogue(const msg_t status) {
+    if (status == MSG_OK) {
+        return I2C_STATUS_SUCCESS;
     }
+
+    // From ChibiOS HAL: "After a timeout the driver must be stopped and
+    // restarted because the bus is in an uncertain state." We also issue that
+    // hard stop in case of any error.
+    i2c_stop();
+
+    return status == MSG_TIMEOUT ? I2C_STATUS_TIMEOUT : I2C_STATUS_ERROR;
 }
 
 __attribute__((weak)) void i2c_init(void) {
@@ -149,14 +158,14 @@ i2c_status_t i2c_transmit(uint8_t address, const uint8_t* data, uint16_t length,
     i2c_address = address;
     i2cStart(&I2C_DRIVER, &i2cconfig);
     msg_t status = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), data, length, 0, 0, TIME_MS2I(timeout));
-    return chibios_to_qmk(&status);
+    return i2c_epilogue(status);
 }
 
 i2c_status_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length, uint16_t timeout) {
     i2c_address = address;
     i2cStart(&I2C_DRIVER, &i2cconfig);
     msg_t status = i2cMasterReceiveTimeout(&I2C_DRIVER, (i2c_address >> 1), data, length, TIME_MS2I(timeout));
-    return chibios_to_qmk(&status);
+    return i2c_epilogue(status);
 }
 
 i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, const uint8_t* data, uint16_t length, uint16_t timeout) {
@@ -170,7 +179,7 @@ i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, const uint8_t* data,
     complete_packet[0] = regaddr;
 
     msg_t status = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), complete_packet, length + 1, 0, 0, TIME_MS2I(timeout));
-    return chibios_to_qmk(&status);
+    return i2c_epilogue(status);
 }
 
 i2c_status_t i2c_writeReg16(uint8_t devaddr, uint16_t regaddr, const uint8_t* data, uint16_t length, uint16_t timeout) {
@@ -185,14 +194,14 @@ i2c_status_t i2c_writeReg16(uint8_t devaddr, uint16_t regaddr, const uint8_t* da
     complete_packet[1] = regaddr & 0xFF;
 
     msg_t status = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), complete_packet, length + 2, 0, 0, TIME_MS2I(timeout));
-    return chibios_to_qmk(&status);
+    return i2c_epilogue(status);
 }
 
 i2c_status_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout) {
     i2c_address = devaddr;
     i2cStart(&I2C_DRIVER, &i2cconfig);
     msg_t status = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), &regaddr, 1, data, length, TIME_MS2I(timeout));
-    return chibios_to_qmk(&status);
+    return i2c_epilogue(status);
 }
 
 i2c_status_t i2c_readReg16(uint8_t devaddr, uint16_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout) {
@@ -200,7 +209,7 @@ i2c_status_t i2c_readReg16(uint8_t devaddr, uint16_t regaddr, uint8_t* data, uin
     i2cStart(&I2C_DRIVER, &i2cconfig);
     uint8_t register_packet[2] = {regaddr >> 8, regaddr & 0xFF};
     msg_t   status             = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), register_packet, 2, data, length, TIME_MS2I(timeout));
-    return chibios_to_qmk(&status);
+    return i2c_epilogue(status);
 }
 
 void i2c_stop(void) {
diff --git a/platforms/chibios/drivers/serial.c b/platforms/chibios/drivers/serial.c
index 0cff057d1d..0dd8e71ae8 100644
--- a/platforms/chibios/drivers/serial.c
+++ b/platforms/chibios/drivers/serial.c
@@ -20,7 +20,8 @@
 #    error "chSysPolledDelayX method not supported on this platform"
 #else
 #    undef wait_us
-#    define wait_us(x) chSysPolledDelayX(US2RTC(CPU_CLOCK, x))
+// Force usage of polled waiting - in case WAIT_US_TIMER is activated
+#    define wait_us(us) chSysPolledDelayX(US2RTC(REALTIME_COUNTER_CLOCK, us))
 #endif
 
 #ifndef SELECT_SOFT_SERIAL_SPEED
@@ -87,10 +88,7 @@ static THD_FUNCTION(Thread1, arg) {
     chRegSetThreadName("blinker");
     while (true) {
         palWaitLineTimeout(SOFT_SERIAL_PIN, TIME_INFINITE);
-
-        split_shared_memory_lock();
         interrupt_handler(NULL);
-        split_shared_memory_unlock();
     }
 }
 
@@ -155,6 +153,7 @@ static void __attribute__((noinline)) serial_write_byte(uint8_t data) {
 
 // interrupt handle to be used by the slave device
 void interrupt_handler(void *arg) {
+    split_shared_memory_lock_autounlock();
     chSysLockFromISR();
 
     sync_send();
@@ -212,6 +211,8 @@ void interrupt_handler(void *arg) {
 static inline bool initiate_transaction(uint8_t sstd_index) {
     if (sstd_index > NUM_TOTAL_TRANSACTIONS) return false;
 
+    split_shared_memory_lock_autounlock();
+
     split_transaction_desc_t *trans = &split_transaction_table[sstd_index];
 
     // TODO: remove extra delay between transactions
@@ -233,7 +234,7 @@ static inline bool initiate_transaction(uint8_t sstd_index) {
     // check if the slave is present
     if (serial_read_pin()) {
         // slave failed to pull the line low, assume not present
-        dprintf("serial::NO_RESPONSE\n");
+        serial_dprintf("serial::NO_RESPONSE\n");
         chSysUnlock();
         return false;
     }
@@ -269,7 +270,7 @@ static inline bool initiate_transaction(uint8_t sstd_index) {
     serial_delay();
 
     if ((checksum_computed) != (checksum_received)) {
-        dprintf("serial::FAIL[%u,%u,%u]\n", checksum_computed, checksum_received, sstd_index);
+        serial_dprintf("serial::FAIL[%u,%u,%u]\n", checksum_computed, checksum_received, sstd_index);
         serial_output();
         serial_high();
 
@@ -292,8 +293,5 @@ static inline bool initiate_transaction(uint8_t sstd_index) {
 //
 // this code is very time dependent, so we need to disable interrupts
 bool soft_serial_transaction(int sstd_index) {
-    split_shared_memory_lock();
-    bool result = initiate_transaction((uint8_t)sstd_index);
-    split_shared_memory_unlock();
-    return result;
+    return initiate_transaction((uint8_t)sstd_index);
 }
diff --git a/platforms/chibios/drivers/serial_protocol.c b/platforms/chibios/drivers/serial_protocol.c
new file mode 100644
index 0000000000..c95aed9885
--- /dev/null
+++ b/platforms/chibios/drivers/serial_protocol.c
@@ -0,0 +1,164 @@
+// Copyright 2022 Stefan Kerkmann
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include <ch.h>
+
+#include "quantum.h"
+#include "serial.h"
+#include "serial_protocol.h"
+#include "printf.h"
+#include "synchronization_util.h"
+
+static inline bool initiate_transaction(uint8_t transaction_id);
+static inline bool react_to_transaction(void);
+
+/**
+ * @brief This thread runs on the slave and responds to transactions initiated
+ * by the master.
+ */
+static THD_WORKING_AREA(waSlaveThread, 1024);
+static THD_FUNCTION(SlaveThread, arg) {
+    (void)arg;
+    chRegSetThreadName("split_protocol_tx_rx");
+
+    while (true) {
+        if (unlikely(!react_to_transaction())) {
+            /* Clear the receive queue, to start with a clean slate.
+             * Parts of failed transactions or spurious bytes could still be in it. */
+            serial_transport_driver_clear();
+        }
+    }
+}
+
+/**
+ * @brief Slave specific initializations.
+ */
+void soft_serial_target_init(void) {
+    serial_transport_driver_slave_init();
+
+    /* Start transport thread. */
+    chThdCreateStatic(waSlaveThread, sizeof(waSlaveThread), HIGHPRIO, SlaveThread, NULL);
+}
+
+/**
+ * @brief Master specific initializations.
+ */
+void soft_serial_initiator_init(void) {
+    serial_transport_driver_master_init();
+}
+
+/**
+ * @brief React to transactions started by the master.
+ */
+static inline bool react_to_transaction(void) {
+    uint8_t transaction_id = 0;
+    /* Wait until there is a transaction for us. */
+    if (unlikely(!serial_transport_receive_blocking(&transaction_id, sizeof(transaction_id)))) {
+        return false;
+    }
+
+    /* Sanity check that we are actually responding to a valid transaction. */
+    if (unlikely(transaction_id >= NUM_TOTAL_TRANSACTIONS)) {
+        return false;
+    }
+
+    split_shared_memory_lock_autounlock();
+
+    split_transaction_desc_t* transaction = &split_transaction_table[transaction_id];
+
+    /* Send back the handshake which is XORed as a simple checksum,
+     to signal that the slave is ready to receive possible transaction buffers  */
+    transaction_id ^= NUM_TOTAL_TRANSACTIONS;
+    if (unlikely(!serial_transport_send(&transaction_id, sizeof(transaction_id)))) {
+        return false;
+    }
+
+    /* Receive transaction buffer from the master. If this transaction requires it.*/
+    if (transaction->initiator2target_buffer_size) {
+        if (unlikely(!serial_transport_receive(split_trans_initiator2target_buffer(transaction), transaction->initiator2target_buffer_size))) {
+            return false;
+        }
+    }
+
+    /* Allow any slave processing to occur. */
+    if (transaction->slave_callback) {
+        transaction->slave_callback(transaction->initiator2target_buffer_size, split_trans_initiator2target_buffer(transaction), transaction->initiator2target_buffer_size, split_trans_target2initiator_buffer(transaction));
+    }
+
+    /* Send transaction buffer to the master. If this transaction requires it. */
+    if (transaction->target2initiator_buffer_size) {
+        if (unlikely(!serial_transport_send(split_trans_target2initiator_buffer(transaction), transaction->target2initiator_buffer_size))) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+/**
+ * @brief Start transaction from the master half to the slave half.
+ *
+ * @param index Transaction Table index of the transaction to start.
+ * @return bool Indicates success of transaction.
+ */
+bool soft_serial_transaction(int index) {
+    bool result = initiate_transaction((uint8_t)index);
+
+    if (unlikely(!result)) {
+        /* Clear the receive queue, to start with a clean slate.
+         * Parts of failed transactions or spurious bytes could still be in it. */
+        serial_transport_driver_clear();
+    }
+
+    return result;
+}
+
+/**
+ * @brief Initiate transaction to slave half.
+ */
+static inline bool initiate_transaction(uint8_t transaction_id) {
+    /* Sanity check that we are actually starting a valid transaction. */
+    if (unlikely(transaction_id >= NUM_TOTAL_TRANSACTIONS)) {
+        serial_dprintf("SPLIT: illegal transaction id\n");
+        return false;
+    }
+
+    split_shared_memory_lock_autounlock();
+
+    split_transaction_desc_t* transaction = &split_transaction_table[transaction_id];
+
+    /* Send transaction table index to the slave, which doubles as basic handshake token. */
+    if (unlikely(!serial_transport_send(&transaction_id, sizeof(transaction_id)))) {
+        serial_dprintf("SPLIT: sending handshake failed\n");
+        return false;
+    }
+
+    uint8_t transaction_id_shake = 0xFF;
+
+    /* Which we always read back first so that we can error out correctly.
+     *   - due to the half duplex limitations on return codes, we always have to read *something*.
+     *   - without the read, write only transactions *always* succeed, even during the boot process where the slave is not ready.
+     */
+    if (unlikely(!serial_transport_receive(&transaction_id_shake, sizeof(transaction_id_shake)) || (transaction_id_shake != (transaction_id ^ NUM_TOTAL_TRANSACTIONS)))) {
+        serial_dprintf("SPLIT: receiving handshake failed\n");
+        return false;
+    }
+
+    /* Send transaction buffer to the slave. If this transaction requires it. */
+    if (transaction->initiator2target_buffer_size) {
+        if (unlikely(!serial_transport_send(split_trans_initiator2target_buffer(transaction), transaction->initiator2target_buffer_size))) {
+            serial_dprintf("SPLIT: sending buffer failed\n");
+            return false;
+        }
+    }
+
+    /* Receive transaction buffer from the slave. If this transaction requires it. */
+    if (transaction->target2initiator_buffer_size) {
+        if (unlikely(!serial_transport_receive(split_trans_target2initiator_buffer(transaction), transaction->target2initiator_buffer_size))) {
+            serial_dprintf("SPLIT: receiving buffer failed\n");
+            return false;
+        }
+    }
+
+    return true;
+}
diff --git a/platforms/chibios/drivers/serial_protocol.h b/platforms/chibios/drivers/serial_protocol.h
new file mode 100644
index 0000000000..4275a7f8d8
--- /dev/null
+++ b/platforms/chibios/drivers/serial_protocol.h
@@ -0,0 +1,49 @@
+// Copyright 2022 Stefan Kerkmann
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#pragma once
+
+/**
+ * @brief Clears any intermediate sending or receiving state of the driver to a known good
+ * state. This happens after errors in the middle of transactions, to start with
+ * a clean slate.
+ */
+void serial_transport_driver_clear(void);
+
+/**
+ * @brief Driver specific initialization on the slave half.
+ */
+void serial_transport_driver_slave_init(void);
+
+/**
+ * @brief Driver specific specific initialization on the master half.
+ */
+void serial_transport_driver_master_init(void);
+
+/**
+ * @brief  Blocking receive of size * bytes.
+ *
+ * @return true Receive success.
+ * @return false Receive failed, e.g. by bit errors.
+ */
+bool __attribute__((nonnull, hot)) serial_transport_receive(uint8_t* destination, const size_t size);
+
+/**
+ * @brief Blocking receive of size * bytes with an implicitly defined timeout.
+ *
+ * @return true Receive success.
+ * @return false Receive failed, e.g. by timeout or bit errors.
+ */
+bool __attribute__((nonnull, hot)) serial_transport_receive_blocking(uint8_t* destination, const size_t size);
+
+/**
+ * @brief Blocking send of buffer with timeout.
+ *
+ * @return true Send success.
+ * @return false Send failed, e.g. by timeout or bit errors.
+ */
+bool __attribute__((nonnull, hot)) serial_transport_send(const uint8_t* source, const size_t size);
diff --git a/platforms/chibios/drivers/serial_usart.c b/platforms/chibios/drivers/serial_usart.c
index e9fa4af7a3..6581a5b6e9 100644
--- a/platforms/chibios/drivers/serial_usart.c
+++ b/platforms/chibios/drivers/serial_usart.c
@@ -1,49 +1,55 @@
-/* Copyright 2021 QMK
- *
- * This program 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 program 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/>.
- */
+// Copyright 2021 QMK
+// Copyright 2022 Stefan Kerkmann
+// SPDX-License-Identifier: GPL-2.0-or-later
 
 #include "serial_usart.h"
+#include "serial_protocol.h"
 #include "synchronization_util.h"
 
 #if defined(SERIAL_USART_CONFIG)
-static SerialConfig serial_config = SERIAL_USART_CONFIG;
-#else
-static SerialConfig serial_config = {
-    .speed = (SERIAL_USART_SPEED), /* speed - mandatory */
+static QMKSerialConfig serial_config = SERIAL_USART_CONFIG;
+#elif defined(MCU_STM32) /* STM32 MCUs */
+static QMKSerialConfig serial_config = {
+#    if HAL_USE_SERIAL
+    .speed = (SERIAL_USART_SPEED),
+#    else
+    .baud = (SERIAL_USART_SPEED),
+#    endif
     .cr1   = (SERIAL_USART_CR1),
     .cr2   = (SERIAL_USART_CR2),
 #    if !defined(SERIAL_USART_FULL_DUPLEX)
     .cr3   = ((SERIAL_USART_CR3) | USART_CR3_HDSEL) /* activate half-duplex mode */
 #    else
-    .cr3 = (SERIAL_USART_CR3)
+    .cr3  = (SERIAL_USART_CR3)
 #    endif
 };
+#elif defined(MCU_RP) /* Raspberry Pi MCUs */
+/* USART in 8E2 config with RX and TX FIFOs enabled. */
+// clang-format off
+static QMKSerialConfig serial_config = {
+    .baud = (SERIAL_USART_SPEED),
+    .UARTLCR_H = UART_UARTLCR_H_WLEN_8BITS | UART_UARTLCR_H_PEN | UART_UARTLCR_H_STP2 | UART_UARTLCR_H_FEN,
+    .UARTCR = 0U,
+    .UARTIFLS = UART_UARTIFLS_RXIFLSEL_1_8F | UART_UARTIFLS_TXIFLSEL_1_8E,
+    .UARTDMACR = 0U
+};
+// clang-format on
+#else
+#    error MCU Familiy not supported by default, supply your own serial_config by defining SERIAL_USART_CONFIG in your keyboard files.
 #endif
 
-static SerialDriver* serial_driver = &SERIAL_USART_DRIVER;
+static QMKSerialDriver* serial_driver = (QMKSerialDriver*)&SERIAL_USART_DRIVER;
 
-static inline bool react_to_transactions(void);
-static inline bool __attribute__((nonnull)) receive(uint8_t* destination, const size_t size);
-static inline bool __attribute__((nonnull)) send(const uint8_t* source, const size_t size);
-static inline bool initiate_transaction(uint8_t sstd_index);
-static inline void usart_clear(void);
+#if HAL_USE_SERIAL
 
 /**
- * @brief Clear the receive input queue.
+ * @brief SERIAL Driver startup routine.
  */
-static inline void usart_clear(void) {
+static inline void usart_driver_start(void) {
+    sdStart(serial_driver, &serial_config);
+}
+
+inline void serial_transport_driver_clear(void) {
     osalSysLock();
     bool volatile queue_not_empty = !iqIsEmptyI(&serial_driver->iqueue);
     osalSysUnlock();
@@ -64,36 +70,96 @@ static inline void usart_clear(void) {
     }
 }
 
+#elif HAL_USE_SIO
+
+void clear_rx_evt_cb(SIODriver* siop) {
+    osalSysLockFromISR();
+    /* If errors occured during transactions this callback is invoked. We just
+     * clear the error sources and move on. We rely on the fact that we check
+     * for the success of the transaction by comparing the received/send bytes
+     * with the actual received/send bytes in the send/receive functions. */
+    sioGetAndClearEventsI(serial_driver);
+    osalSysUnlockFromISR();
+}
+
+static const SIOOperation serial_usart_operation = {.rx_cb = NULL, .rx_idle_cb = NULL, .tx_cb = NULL, .tx_end_cb = NULL, .rx_evt_cb = &clear_rx_evt_cb};
+
 /**
- * @brief Blocking send of buffer with timeout.
- *
- * @return true Send success.
- * @return false Send failed.
+ * @brief SIO Driver startup routine.
  */
-static inline bool send(const uint8_t* source, const size_t size) {
-    bool success = (size_t)sdWriteTimeout(serial_driver, source, size, TIME_MS2I(SERIAL_USART_TIMEOUT)) == size;
+static inline void usart_driver_start(void) {
+    sioStart(serial_driver, &serial_config);
+    sioStartOperation(serial_driver, &serial_usart_operation);
+}
+
+inline void serial_transport_driver_clear(void) {
+    osalSysLock();
+    while (!sioIsRXEmptyX(serial_driver)) {
+        (void)sioGetX(serial_driver);
+    }
+    osalSysUnlock();
+}
+
+#else
+
+#    error Either the SERIAL or SIO driver has to be activated to use the usart driver for split keyboards.
+
+#endif
+
+inline bool serial_transport_send(const uint8_t* source, const size_t size) {
+    bool success = (size_t)chnWriteTimeout(serial_driver, source, size, TIME_MS2I(SERIAL_USART_TIMEOUT)) == size;
 
 #if !defined(SERIAL_USART_FULL_DUPLEX)
-    if (success) {
-        /* Half duplex fills the input queue with the data we wrote - just throw it away.
-           Under the right circumstances (e.g. bad cables paired with high baud rates)
-           less bytes can be present in the input queue, therefore a timeout is needed. */
-        uint8_t dump[size];
-        return receive(dump, size);
+    /* Half duplex fills the input queue with the data we wrote - just throw it away. */
+    if (likely(success)) {
+        size_t bytes_left = size;
+#    if HAL_USE_SERIAL
+        /* The SERIAL driver uses large soft FIFOs that are filled from an IRQ
+         * context, so there is a delay between receiving the data and it
+         * becoming actually available, therefore we have to apply a timeout
+         * mechanism. Under the right circumstances (e.g. bad cables paired with
+         * high baud rates) less bytes can be present in the input queue as
+         * well. */
+        uint8_t dump[64];
+
+        while (unlikely(bytes_left >= 64)) {
+            if (unlikely(!serial_transport_receive(dump, 64))) {
+                return false;
+            }
+            bytes_left -= 64;
+        }
+
+        return serial_transport_receive(dump, bytes_left);
+#    else
+        /* The SIO driver directly accesses the hardware FIFOs of the USART
+         * peripheral. As these are limited in depth, the RX FIFO might have been
+         * overflowed by a large that we just send. Therefore we attempt to read
+         * back all the data we send or until the FIFO runs empty in case it
+         * overflowed and data was truncated. */
+        if (unlikely(sioSynchronizeTXEnd(serial_driver, TIME_MS2I(SERIAL_USART_TIMEOUT)) < MSG_OK)) {
+            return false;
+        }
+
+        osalSysLock();
+        while (bytes_left > 0 && !sioIsRXEmptyX(serial_driver)) {
+            (void)sioGetX(serial_driver);
+            bytes_left--;
+        }
+        osalSysUnlock();
+#    endif
     }
 #endif
 
     return success;
 }
 
-/**
- * @brief  Blocking receive of size * bytes with timeout.
- *
- * @return true Receive success.
- * @return false Receive failed.
- */
-static inline bool receive(uint8_t* destination, const size_t size) {
-    bool success = (size_t)sdReadTimeout(serial_driver, destination, size, TIME_MS2I(SERIAL_USART_TIMEOUT)) == size;
+inline bool serial_transport_receive(uint8_t* destination, const size_t size) {
+    bool success = (size_t)chnReadTimeout(serial_driver, destination, size, TIME_MS2I(SERIAL_USART_TIMEOUT)) == size;
+    return success;
+}
+
+inline bool serial_transport_receive_blocking(uint8_t* destination, const size_t size) {
+    bool success = (size_t)chnRead(serial_driver, destination, size) == size;
     return success;
 }
 
@@ -103,7 +169,7 @@ static inline bool receive(uint8_t* destination, const size_t size) {
  * @brief Initiate pins for USART peripheral. Half-duplex configuration.
  */
 __attribute__((weak)) void usart_init(void) {
-#    if defined(MCU_STM32)
+#    if defined(MCU_STM32) /* STM32 MCUs */
 #        if defined(USE_GPIOV1)
     palSetLineMode(SERIAL_USART_TX_PIN, PAL_MODE_ALTERNATE_OPENDRAIN);
 #        else
@@ -113,6 +179,8 @@ __attribute__((weak)) void usart_init(void) {
 #        if defined(USART_REMAP)
     USART_REMAP;
 #        endif
+#    elif defined(MCU_RP) /* Raspberry Pi MCUs */
+#        error Half-duplex with the SIO driver is not supported due to hardware limitations on the RP2040, switch to the PIO driver which has half-duplex support.
 #    else
 #        pragma message "usart_init: MCU Familiy not supported by default, please supply your own init code by implementing usart_init() in your keyboard files."
 #    endif
@@ -124,7 +192,7 @@ __attribute__((weak)) void usart_init(void) {
  * @brief Initiate pins for USART peripheral. Full-duplex configuration.
  */
 __attribute__((weak)) void usart_init(void) {
-#    if defined(MCU_STM32)
+#    if defined(MCU_STM32) /* STM32 MCUs */
 #        if defined(USE_GPIOV1)
     palSetLineMode(SERIAL_USART_TX_PIN, PAL_MODE_ALTERNATE_PUSHPULL);
     palSetLineMode(SERIAL_USART_RX_PIN, PAL_MODE_INPUT);
@@ -136,6 +204,9 @@ __attribute__((weak)) void usart_init(void) {
 #        if defined(USART_REMAP)
     USART_REMAP;
 #        endif
+#    elif defined(MCU_RP) /* Raspberry Pi MCUs */
+    palSetLineMode(SERIAL_USART_TX_PIN, PAL_MODE_ALTERNATE_UART);
+    palSetLineMode(SERIAL_USART_RX_PIN, PAL_MODE_ALTERNATE_UART);
 #    else
 #        pragma message "usart_init: MCU Familiy not supported by default, please supply your own init code by implementing usart_init() in your keyboard files."
 #    endif
@@ -146,7 +217,7 @@ __attribute__((weak)) void usart_init(void) {
 /**
  * @brief Overridable master specific initializations.
  */
-__attribute__((weak, nonnull)) void usart_master_init(SerialDriver** driver) {
+__attribute__((weak, nonnull)) void usart_master_init(QMKSerialDriver** driver) {
     (void)driver;
     usart_init();
 }
@@ -154,161 +225,22 @@ __attribute__((weak, nonnull)) void usart_master_init(SerialDriver** driver) {
 /**
  * @brief Overridable slave specific initializations.
  */
-__attribute__((weak, nonnull)) void usart_slave_init(SerialDriver** driver) {
+__attribute__((weak, nonnull)) void usart_slave_init(QMKSerialDriver** driver) {
     (void)driver;
     usart_init();
 }
 
-/**
- * @brief This thread runs on the slave and responds to transactions initiated
- * by the master.
- */
-static THD_WORKING_AREA(waSlaveThread, 1024);
-static THD_FUNCTION(SlaveThread, arg) {
-    (void)arg;
-    chRegSetThreadName("usart_tx_rx");
-
-    while (true) {
-        if (!react_to_transactions()) {
-            /* Clear the receive queue, to start with a clean slate.
-             * Parts of failed transactions or spurious bytes could still be in it. */
-            usart_clear();
-        }
-        split_shared_memory_unlock();
-    }
-}
-
-/**
- * @brief Slave specific initializations.
- */
-void soft_serial_target_init(void) {
+void serial_transport_driver_slave_init(void) {
     usart_slave_init(&serial_driver);
-
-    sdStart(serial_driver, &serial_config);
-
-    /* Start transport thread. */
-    chThdCreateStatic(waSlaveThread, sizeof(waSlaveThread), HIGHPRIO, SlaveThread, NULL);
+    usart_driver_start();
 }
 
-/**
- * @brief React to transactions started by the master.
- */
-static inline bool react_to_transactions(void) {
-    /* Wait until there is a transaction for us. */
-    uint8_t sstd_index = (uint8_t)sdGet(serial_driver);
-
-    /* Sanity check that we are actually responding to a valid transaction. */
-    if (sstd_index >= NUM_TOTAL_TRANSACTIONS) {
-        return false;
-    }
-
-    split_shared_memory_lock();
-    split_transaction_desc_t* trans = &split_transaction_table[sstd_index];
-
-    /* Send back the handshake which is XORed as a simple checksum,
-     to signal that the slave is ready to receive possible transaction buffers  */
-    sstd_index ^= HANDSHAKE_MAGIC;
-    if (!send(&sstd_index, sizeof(sstd_index))) {
-        return false;
-    }
-
-    /* Receive transaction buffer from the master. If this transaction requires it.*/
-    if (trans->initiator2target_buffer_size) {
-        if (!receive(split_trans_initiator2target_buffer(trans), trans->initiator2target_buffer_size)) {
-            return false;
-        }
-    }
-
-    /* Allow any slave processing to occur. */
-    if (trans->slave_callback) {
-        trans->slave_callback(trans->initiator2target_buffer_size, split_trans_initiator2target_buffer(trans), trans->initiator2target_buffer_size, split_trans_target2initiator_buffer(trans));
-    }
-
-    /* Send transaction buffer to the master. If this transaction requires it. */
-    if (trans->target2initiator_buffer_size) {
-        if (!send(split_trans_target2initiator_buffer(trans), trans->target2initiator_buffer_size)) {
-            return false;
-        }
-    }
-
-    return true;
-}
-
-/**
- * @brief Master specific initializations.
- */
-void soft_serial_initiator_init(void) {
+void serial_transport_driver_master_init(void) {
     usart_master_init(&serial_driver);
 
 #if defined(MCU_STM32) && defined(SERIAL_USART_PIN_SWAP)
     serial_config.cr2 |= USART_CR2_SWAP; // master has swapped TX/RX pins
 #endif
 
-    sdStart(serial_driver, &serial_config);
-}
-
-/**
- * @brief Start transaction from the master half to the slave half.
- *
- * @param index Transaction Table index of the transaction to start.
- * @return bool Indicates success of transaction.
- */
-bool soft_serial_transaction(int index) {
-    /* Clear the receive queue, to start with a clean slate.
-     * Parts of failed transactions or spurious bytes could still be in it. */
-    usart_clear();
-
-    split_shared_memory_lock();
-    bool result = initiate_transaction((uint8_t)index);
-    split_shared_memory_unlock();
-
-    return result;
-}
-
-/**
- * @brief Initiate transaction to slave half.
- */
-static inline bool initiate_transaction(uint8_t sstd_index) {
-    /* Sanity check that we are actually starting a valid transaction. */
-    if (sstd_index >= NUM_TOTAL_TRANSACTIONS) {
-        dprintln("USART: Illegal transaction Id.");
-        return false;
-    }
-
-    split_transaction_desc_t* trans = &split_transaction_table[sstd_index];
-
-    /* Send transaction table index to the slave, which doubles as basic handshake token. */
-    if (!send(&sstd_index, sizeof(sstd_index))) {
-        dprintln("USART: Send Handshake failed.");
-        return false;
-    }
-
-    uint8_t sstd_index_shake = 0xFF;
-
-    /* Which we always read back first so that we can error out correctly.
-     *   - due to the half duplex limitations on return codes, we always have to read *something*.
-     *   - without the read, write only transactions *always* succeed, even during the boot process where the slave is not ready.
-     */
-    if (!receive(&sstd_index_shake, sizeof(sstd_index_shake)) || (sstd_index_shake != (sstd_index ^ HANDSHAKE_MAGIC))) {
-        dprintln("USART: Handshake failed.");
-        return false;
-    }
-
-    /* Send transaction buffer to the slave. If this transaction requires it. */
-    if (trans->initiator2target_buffer_size) {
-        if (!send(split_trans_initiator2target_buffer(trans), trans->initiator2target_buffer_size)) {
-            dprintln("USART: Send failed.");
-            return false;
-        }
-    }
-
-    /* Receive transaction buffer from the slave. If this transaction requires it. */
-    if (trans->target2initiator_buffer_size) {
-        if (!receive(split_trans_target2initiator_buffer(trans), trans->target2initiator_buffer_size)) {
-            dprintln("USART: Receive failed.");
-            return false;
-        }
-    }
-
-    return true;
+    usart_driver_start();
 }
diff --git a/platforms/chibios/drivers/serial_usart.h b/platforms/chibios/drivers/serial_usart.h
index 81fe9e0113..fa062cd736 100644
--- a/platforms/chibios/drivers/serial_usart.h
+++ b/platforms/chibios/drivers/serial_usart.h
@@ -1,42 +1,12 @@
-/* Copyright 2021 QMK
- *
- * This program 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 program 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/>.
- */
+// Copyright 2021 QMK
+// SPDX-License-Identifier: GPL-2.0-or-later
 
 #pragma once
 
 #include "quantum.h"
 #include "serial.h"
-#include "printf.h"
-
-#include <ch.h>
 #include <hal.h>
 
-#if !defined(SERIAL_USART_DRIVER)
-#    define SERIAL_USART_DRIVER SD1
-#endif
-
-#if !defined(USE_GPIOV1)
-/* The default PAL alternate modes are used to signal that the pins are used for USART. */
-#    if !defined(SERIAL_USART_TX_PAL_MODE)
-#        define SERIAL_USART_TX_PAL_MODE 7
-#    endif
-#    if !defined(SERIAL_USART_RX_PAL_MODE)
-#        define SERIAL_USART_RX_PAL_MODE 7
-#    endif
-#endif
-
 #if defined(SOFT_SERIAL_PIN)
 #    define SERIAL_USART_TX_PIN SOFT_SERIAL_PIN
 #endif
@@ -49,6 +19,62 @@
 #    define SERIAL_USART_RX_PIN A10
 #endif
 
+#if !defined(SELECT_SOFT_SERIAL_SPEED)
+#    define SELECT_SOFT_SERIAL_SPEED 1
+#endif
+
+#if defined(SERIAL_USART_SPEED)
+// Allow advanced users to directly set SERIAL_USART_SPEED
+#elif SELECT_SOFT_SERIAL_SPEED == 0
+#    define SERIAL_USART_SPEED 460800
+#elif SELECT_SOFT_SERIAL_SPEED == 1
+#    define SERIAL_USART_SPEED 230400
+#elif SELECT_SOFT_SERIAL_SPEED == 2
+#    define SERIAL_USART_SPEED 115200
+#elif SELECT_SOFT_SERIAL_SPEED == 3
+#    define SERIAL_USART_SPEED 57600
+#elif SELECT_SOFT_SERIAL_SPEED == 4
+#    define SERIAL_USART_SPEED 38400
+#elif SELECT_SOFT_SERIAL_SPEED == 5
+#    define SERIAL_USART_SPEED 19200
+#else
+#    error invalid SELECT_SOFT_SERIAL_SPEED value
+#endif
+
+#if !defined(SERIAL_USART_TIMEOUT)
+#    define SERIAL_USART_TIMEOUT 20
+#endif
+
+#if HAL_USE_SERIAL
+
+typedef SerialDriver QMKSerialDriver;
+typedef SerialConfig QMKSerialConfig;
+
+#    if !defined(SERIAL_USART_DRIVER)
+#        define SERIAL_USART_DRIVER SD1
+#    endif
+
+#elif HAL_USE_SIO
+
+typedef SIODriver QMKSerialDriver;
+typedef SIOConfig QMKSerialConfig;
+
+#    if !defined(SERIAL_USART_DRIVER)
+#        define SERIAL_USART_DRIVER SIOD1
+#    endif
+
+#endif
+
+#if !defined(USE_GPIOV1)
+/* The default PAL alternate modes are used to signal that the pins are used for USART. */
+#    if !defined(SERIAL_USART_TX_PAL_MODE)
+#        define SERIAL_USART_TX_PAL_MODE 7
+#    endif
+#    if !defined(SERIAL_USART_RX_PAL_MODE)
+#        define SERIAL_USART_RX_PAL_MODE 7
+#    endif
+#endif
+
 #if !defined(USART_CR1_M0)
 #    define USART_CR1_M0 USART_CR1_M // some platforms (f1xx) dont have this so
 #endif
@@ -86,31 +112,3 @@
             (AFIO->MAPR |= AFIO_MAPR_USART3_REMAP_FULLREMAP); \
         } while (0)
 #endif
-
-#if !defined(SELECT_SOFT_SERIAL_SPEED)
-#    define SELECT_SOFT_SERIAL_SPEED 1
-#endif
-
-#if defined(SERIAL_USART_SPEED)
-// Allow advanced users to directly set SERIAL_USART_SPEED
-#elif SELECT_SOFT_SERIAL_SPEED == 0
-#    define SERIAL_USART_SPEED 460800
-#elif SELECT_SOFT_SERIAL_SPEED == 1
-#    define SERIAL_USART_SPEED 230400
-#elif SELECT_SOFT_SERIAL_SPEED == 2
-#    define SERIAL_USART_SPEED 115200
-#elif SELECT_SOFT_SERIAL_SPEED == 3
-#    define SERIAL_USART_SPEED 57600
-#elif SELECT_SOFT_SERIAL_SPEED == 4
-#    define SERIAL_USART_SPEED 38400
-#elif SELECT_SOFT_SERIAL_SPEED == 5
-#    define SERIAL_USART_SPEED 19200
-#else
-#    error invalid SELECT_SOFT_SERIAL_SPEED value
-#endif
-
-#if !defined(SERIAL_USART_TIMEOUT)
-#    define SERIAL_USART_TIMEOUT 20
-#endif
-
-#define HANDSHAKE_MAGIC 7
diff --git a/platforms/chibios/drivers/spi_master.c b/platforms/chibios/drivers/spi_master.c
index ce69e7f0ac..c3ab0623f0 100644
--- a/platforms/chibios/drivers/spi_master.c
+++ b/platforms/chibios/drivers/spi_master.c
@@ -20,7 +20,7 @@
 
 static pin_t currentSlavePin = NO_PIN;
 
-#if defined(K20x) || defined(KL2x)
+#if defined(K20x) || defined(KL2x) || defined(RP2040)
 static SPIConfig spiConfig = {NULL, 0, 0, 0};
 #else
 static SPIConfig spiConfig = {false, NULL, 0, 0, 0, 0};
@@ -42,10 +42,12 @@ __attribute__((weak)) void spi_init(void) {
         palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), SPI_MOSI_PAL_MODE);
         palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), SPI_MISO_PAL_MODE);
 #else
-        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_ALTERNATE(SPI_SCK_PAL_MODE) | PAL_OUTPUT_TYPE_PUSHPULL | PAL_OUTPUT_SPEED_HIGHEST);
-        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_ALTERNATE(SPI_MOSI_PAL_MODE) | PAL_OUTPUT_TYPE_PUSHPULL | PAL_OUTPUT_SPEED_HIGHEST);
-        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_ALTERNATE(SPI_MISO_PAL_MODE) | PAL_OUTPUT_TYPE_PUSHPULL | PAL_OUTPUT_SPEED_HIGHEST);
+        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), SPI_SCK_FLAGS);
+        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), SPI_MOSI_FLAGS);
+        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), SPI_MISO_FLAGS);
 #endif
+        spiStop(&SPI_DRIVER);
+        currentSlavePin = NO_PIN;
     }
 }
 
@@ -167,7 +169,36 @@ bool spi_start(pin_t slavePin, bool lsbFirst, uint8_t mode, uint16_t divisor) {
             spiConfig.SPI_CPOL = SPI_CPOL_High;
             break;
     }
+#elif defined(MCU_RP)
+    if (lsbFirst) {
+        osalDbgAssert(lsbFirst == false, "RP2040s PrimeCell SPI implementation does not support sending LSB first.");
+    }
+
+    // Motorola frame format and 8bit transfer data size.
+    spiConfig.SSPCR0 = SPI_SSPCR0_FRF_MOTOROLA | SPI_SSPCR0_DSS_8BIT;
+    // Serial output clock = (ck_sys or ck_peri) / (SSPCPSR->CPSDVSR * (1 +
+    // SSPCR0->SCR)). SCR is always set to zero, as QMK SPI API expects the
+    // passed divisor to be the only value to divide the input clock by.
+    spiConfig.SSPCPSR = roundedDivisor; // Even number from 2 to 254
 
+    switch (mode) {
+        case 0:
+            spiConfig.SSPCR0 &= ~SPI_SSPCR0_SPO; // Clock polarity: low
+            spiConfig.SSPCR0 &= ~SPI_SSPCR0_SPH; // Clock phase: sample on first edge
+            break;
+        case 1:
+            spiConfig.SSPCR0 &= ~SPI_SSPCR0_SPO; // Clock polarity: low
+            spiConfig.SSPCR0 |= SPI_SSPCR0_SPH;  // Clock phase: sample on second edge transition
+            break;
+        case 2:
+            spiConfig.SSPCR0 |= SPI_SSPCR0_SPO;  // Clock polarity: high
+            spiConfig.SSPCR0 &= ~SPI_SSPCR0_SPH; // Clock phase: sample on first edge
+            break;
+        case 3:
+            spiConfig.SSPCR0 |= SPI_SSPCR0_SPO; // Clock polarity: high
+            spiConfig.SSPCR0 |= SPI_SSPCR0_SPH; // Clock phase: sample on second edge transition
+            break;
+    }
 #else
     spiConfig.cr1 = 0;
 
diff --git a/platforms/chibios/drivers/vendor/RP/RP2040/serial_vendor.c b/platforms/chibios/drivers/vendor/RP/RP2040/serial_vendor.c
new file mode 100644
index 0000000000..764764b3f9
--- /dev/null
+++ b/platforms/chibios/drivers/vendor/RP/RP2040/serial_vendor.c
@@ -0,0 +1,473 @@
+// Copyright 2022 Stefan Kerkmann
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include "quantum.h"
+#include "serial_usart.h"
+#include "serial_protocol.h"
+#include "hardware/pio.h"
+#include "hardware/clocks.h"
+
+#if !defined(MCU_RP)
+#    error PIO Driver is only available for Raspberry Pi 2040 MCUs!
+#endif
+
+static inline bool receive_impl(uint8_t* destination, const size_t size, sysinterval_t timeout);
+static inline bool send_impl(const uint8_t* source, const size_t size);
+static inline void pio_serve_interrupt(void);
+
+#define MSG_PIO_ERROR ((msg_t)(-3))
+
+#if defined(SERIAL_PIO_USE_PIO1)
+static const PIO pio = pio1;
+
+OSAL_IRQ_HANDLER(RP_PIO1_IRQ_0_HANDLER) {
+    OSAL_IRQ_PROLOGUE();
+    pio_serve_interrupt();
+    OSAL_IRQ_EPILOGUE();
+}
+#else
+static const PIO pio = pio0;
+
+OSAL_IRQ_HANDLER(RP_PIO0_IRQ_0_HANDLER) {
+    OSAL_IRQ_PROLOGUE();
+    pio_serve_interrupt();
+    OSAL_IRQ_EPILOGUE();
+}
+#endif
+
+#define UART_TX_WRAP_TARGET 0
+#define UART_TX_WRAP 3
+
+// clang-format off
+#if defined(SERIAL_USART_FULL_DUPLEX)
+static const uint16_t uart_tx_program_instructions[] = {
+            //     .wrap_target
+    0x9fa0, //  0: pull   block           side 1 [7]
+    0xf727, //  1: set    x, 7            side 0 [7]
+    0x6001, //  2: out    pins, 1
+    0x0642, //  3: jmp    x--, 2                 [6]
+            //     .wrap
+};
+#else
+static const uint16_t uart_tx_program_instructions[] = {
+            //     .wrap_target
+    0x9fa0, //  0: pull   block           side 1 [7]
+    0xf727, //  1: set    x, 7            side 0 [7]
+    0x6081, //  2: out    pindirs, 1
+    0x0642, //  3: jmp    x--, 2                 [6]
+            //     .wrap
+};
+#endif
+// clang-format on
+
+static const pio_program_t uart_tx_program = {
+    .instructions = uart_tx_program_instructions,
+    .length       = 4,
+    .origin       = -1,
+};
+
+#define UART_RX_WRAP_TARGET 0
+#define UART_RX_WRAP 8
+
+// clang-format off
+static const uint16_t uart_rx_program_instructions[] = {
+            //     .wrap_target
+    0x2020, //  0: wait   0 pin, 0
+    0xea27, //  1: set    x, 7                   [10]
+    0x4001, //  2: in     pins, 1
+    0x0642, //  3: jmp    x--, 2                 [6]
+    0x00c8, //  4: jmp    pin, 8
+    0xc020, //  5: irq    wait 0
+    0x20a0, //  6: wait   1 pin, 0
+    0x0000, //  7: jmp    0
+    0x8020, //  8: push   block
+            //     .wrap
+};
+// clang-format on
+
+static const pio_program_t uart_rx_program = {
+    .instructions = uart_rx_program_instructions,
+    .length       = 9,
+    .origin       = -1,
+};
+
+thread_reference_t rx_thread        = NULL;
+static int         rx_state_machine = -1;
+
+thread_reference_t tx_thread        = NULL;
+static int         tx_state_machine = -1;
+
+void pio_serve_interrupt(void) {
+    uint32_t irqs = pio->ints0;
+
+    // The RX FIFO is not empty any more, therefore wake any sleeping rx thread
+    if (irqs & (PIO_IRQ0_INTF_SM0_RXNEMPTY_BITS << rx_state_machine)) {
+        // Disable rx not empty interrupt
+        pio_set_irq0_source_enabled(pio, pis_sm0_rx_fifo_not_empty + rx_state_machine, false);
+
+        osalSysLockFromISR();
+        osalThreadResumeI(&rx_thread, MSG_OK);
+        osalSysUnlockFromISR();
+    }
+
+    // The TX FIFO is not full any more, therefore wake any sleeping tx thread
+    if (irqs & (PIO_IRQ0_INTF_SM0_TXNFULL_BITS << tx_state_machine)) {
+        // Disable tx not full interrupt
+        pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + tx_state_machine, false);
+        osalSysLockFromISR();
+        osalThreadResumeI(&tx_thread, MSG_OK);
+        osalSysUnlockFromISR();
+    }
+
+    // IRQ 0 is set on framing or break errors by the rx state machine
+    if (pio_interrupt_get(pio, 0UL)) {
+        pio_interrupt_clear(pio, 0UL);
+
+        osalSysLockFromISR();
+        osalThreadResumeI(&rx_thread, MSG_PIO_ERROR);
+        osalSysUnlockFromISR();
+    }
+}
+
+#if !defined(SERIAL_USART_FULL_DUPLEX)
+// The internal pull-ups of the RP2040 are rather weakish with a range of 50k to
+// 80k, which in turn do not provide enough current to guarantee fast signal rise
+// times with a parasitic capacitance of greater than 100pf. In real world
+// applications, like split keyboards which might have vias in the signal path
+// or long PCB traces, this prevents a successful communication. The solution
+// is to temporarily augment the weak pull ups from the receiving side by
+// driving the tx pin high. On the receiving side the lowest possible drive
+// strength is chosen because the transmitting side must still be able to drive
+// the signal low. With this configuration the rise times are fast enough and
+// the generated low level with 360mV will generate a logical zero.
+static inline void enter_rx_state(void) {
+    osalSysLock();
+    nvicEnableVector(RP_USBCTRL_IRQ_NUMBER, RP_IRQ_USB0_PRIORITY);
+    // Wait for the transmitting state machines FIFO to run empty. At this point
+    // the last byte has been pulled from the transmitting state machines FIFO
+    // into the output shift register. We have to wait a tiny bit more until
+    // this byte is transmitted, before we can turn on the receiving state
+    // machine again.
+    while (!pio_sm_is_tx_fifo_empty(pio, tx_state_machine)) {
+    }
+    // Wait for ~11 bits, 1 start bit + 8 data bits + 1 stop bit + 1 bit
+    // headroom.
+    wait_us(1000000U * 11U / SERIAL_USART_SPEED);
+    // Disable tx state machine to not interfere with our tx pin manipulation
+    pio_sm_set_enabled(pio, tx_state_machine, false);
+    gpio_set_drive_strength(SERIAL_USART_TX_PIN, GPIO_DRIVE_STRENGTH_2MA);
+    pio_sm_set_pins_with_mask(pio, tx_state_machine, 1U << SERIAL_USART_TX_PIN, 1U << SERIAL_USART_TX_PIN);
+    pio_sm_set_consecutive_pindirs(pio, tx_state_machine, SERIAL_USART_TX_PIN, 1U, false);
+    pio_sm_set_enabled(pio, rx_state_machine, true);
+    osalSysUnlock();
+}
+
+static inline void leave_rx_state(void) {
+    osalSysLock();
+    // We don't want to be interrupted by frequent (1KHz) USB interrupts while
+    // doing our timing critical sending operation.
+    nvicDisableVector(RP_USBCTRL_IRQ_NUMBER);
+    // In Half-duplex operation the tx pin dual-functions as sender and
+    // receiver. To not receive the data we will send, we disable the receiving
+    // state machine.
+    pio_sm_set_enabled(pio, rx_state_machine, false);
+    pio_sm_set_consecutive_pindirs(pio, tx_state_machine, SERIAL_USART_TX_PIN, 1U, true);
+    pio_sm_set_pins_with_mask(pio, tx_state_machine, 0U, 1U << SERIAL_USART_TX_PIN);
+    gpio_set_drive_strength(SERIAL_USART_TX_PIN, GPIO_DRIVE_STRENGTH_12MA);
+    pio_sm_restart(pio, tx_state_machine);
+    pio_sm_set_enabled(pio, tx_state_machine, true);
+    osalSysUnlock();
+}
+#else
+// All this trickery is gladly not necessary for full-duplex.
+static inline void enter_rx_state(void) {}
+static inline void leave_rx_state(void) {}
+#endif
+
+/**
+ * @brief Clear the RX and TX hardware FIFOs of the state machines.
+ */
+inline void serial_transport_driver_clear(void) {
+    osalSysLock();
+    pio_sm_clear_fifos(pio, rx_state_machine);
+    pio_sm_clear_fifos(pio, tx_state_machine);
+    osalSysUnlock();
+}
+
+static inline msg_t sync_tx(sysinterval_t timeout) {
+    msg_t msg = MSG_OK;
+    osalSysLock();
+    while (pio_sm_is_tx_fifo_full(pio, tx_state_machine)) {
+#if !defined(SERIAL_USART_FULL_DUPLEX)
+        // Enable USB interrupts again, because we might sleep for a long time
+        // here and don't want to be disconnected from the host.
+        nvicEnableVector(RP_USBCTRL_IRQ_NUMBER, RP_IRQ_USB0_PRIORITY);
+#endif
+        pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + tx_state_machine, true);
+        msg = osalThreadSuspendTimeoutS(&tx_thread, timeout);
+        if (msg < MSG_OK) {
+            pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + tx_state_machine, false);
+            break;
+        }
+    }
+#if !defined(SERIAL_USART_FULL_DUPLEX)
+    // Entering timing critical territory again.
+    nvicDisableVector(RP_USBCTRL_IRQ_NUMBER);
+#endif
+    osalSysUnlock();
+    return msg;
+}
+
+static inline bool send_impl(const uint8_t* source, const size_t size) {
+    size_t send = 0;
+    msg_t  msg;
+    while (send < size) {
+        msg = sync_tx(TIME_MS2I(SERIAL_USART_TIMEOUT));
+        if (msg < MSG_OK) {
+            return false;
+        }
+
+        osalSysLock();
+        while (send < size) {
+            if (pio_sm_is_tx_fifo_full(pio, tx_state_machine)) {
+                break;
+            }
+            if (send >= size) {
+                break;
+            }
+            pio_sm_put(pio, tx_state_machine, (uint32_t)(*source));
+            source++;
+            send++;
+        }
+        osalSysUnlock();
+    }
+
+    return send == size;
+}
+
+/**
+ * @brief Blocking send of buffer with timeout.
+ *
+ * @return true Send success.
+ * @return false Send failed.
+ */
+inline bool serial_transport_send(const uint8_t* source, const size_t size) {
+    leave_rx_state();
+    bool result = send_impl(source, size);
+    enter_rx_state();
+
+    return result;
+}
+
+static inline msg_t sync_rx(sysinterval_t timeout) {
+    msg_t msg = MSG_OK;
+    osalSysLock();
+    while (pio_sm_is_rx_fifo_empty(pio, rx_state_machine)) {
+        pio_set_irq0_source_enabled(pio, pis_sm0_rx_fifo_not_empty + rx_state_machine, true);
+        msg = osalThreadSuspendTimeoutS(&rx_thread, timeout);
+        if (msg < MSG_OK) {
+            pio_set_irq0_source_enabled(pio, pis_sm0_rx_fifo_not_empty + rx_state_machine, false);
+            break;
+        }
+    }
+    osalSysUnlock();
+    return msg;
+}
+
+static inline bool receive_impl(uint8_t* destination, const size_t size, sysinterval_t timeout) {
+    size_t read = 0U;
+
+    while (read < size) {
+        msg_t msg = sync_rx(timeout);
+        if (msg < MSG_OK) {
+            return false;
+        }
+        osalSysLock();
+        while (true) {
+            if (pio_sm_is_rx_fifo_empty(pio, rx_state_machine)) {
+                break;
+            }
+            if (read >= size) {
+                break;
+            }
+            *destination++ = *((uint8_t*)&pio->rxf[rx_state_machine] + 3U);
+            read++;
+        }
+        osalSysUnlock();
+    }
+
+    return read == size;
+}
+
+/**
+ * @brief  Blocking receive of size * bytes with timeout.
+ *
+ * @return true Receive success.
+ * @return false Receive failed, e.g. by timeout.
+ */
+inline bool serial_transport_receive(uint8_t* destination, const size_t size) {
+    return receive_impl(destination, size, TIME_MS2I(SERIAL_USART_TIMEOUT));
+}
+
+/**
+ * @brief  Blocking receive of size * bytes.
+ *
+ * @return true Receive success.
+ * @return false Receive failed.
+ */
+inline bool serial_transport_receive_blocking(uint8_t* destination, const size_t size) {
+    return receive_impl(destination, size, TIME_INFINITE);
+}
+
+static inline void pio_tx_init(pin_t tx_pin) {
+    uint pio_idx = pio_get_index(pio);
+    uint offset  = pio_add_program(pio, &uart_tx_program);
+
+#if defined(SERIAL_USART_FULL_DUPLEX)
+    // clang-format off
+    iomode_t tx_pin_mode = PAL_RP_GPIO_OE |
+                           PAL_RP_PAD_SLEWFAST |
+                           PAL_RP_PAD_DRIVE4 |
+                           (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1);
+    // clang-format on
+    pio_sm_set_pins_with_mask(pio, tx_state_machine, 1U << tx_pin, 1U << tx_pin);
+    pio_sm_set_consecutive_pindirs(pio, tx_state_machine, tx_pin, 1U, true);
+#else
+    // clang-format off
+    iomode_t tx_pin_mode = PAL_RP_PAD_IE |
+                           PAL_RP_GPIO_OE |
+                           PAL_RP_PAD_SCHMITT |
+                           PAL_RP_PAD_PUE |
+                           PAL_RP_PAD_SLEWFAST |
+                           PAL_RP_PAD_DRIVE12 |
+                           PAL_RP_IOCTRL_OEOVER_DRVINVPERI |
+                           (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1);
+    // clang-format on
+    pio_sm_set_pins_with_mask(pio, tx_state_machine, 0U << tx_pin, 1U << tx_pin);
+    pio_sm_set_consecutive_pindirs(pio, tx_state_machine, tx_pin, 1U, true);
+#endif
+
+    palSetLineMode(tx_pin, tx_pin_mode);
+
+    pio_sm_config config = pio_get_default_sm_config();
+    sm_config_set_wrap(&config, offset + UART_TX_WRAP_TARGET, offset + UART_TX_WRAP);
+#if defined(SERIAL_USART_FULL_DUPLEX)
+    sm_config_set_sideset(&config, 2, true, false);
+#else
+    sm_config_set_sideset(&config, 2, true, true);
+#endif
+    // OUT shifts to right, no autopull
+    sm_config_set_out_shift(&config, true, false, 32);
+    // We are mapping both OUT and side-set to the same pin, because sometimes
+    // we need to assert user data onto the pin (with OUT) and sometimes
+    // assert constant values (start/stop bit)
+    sm_config_set_out_pins(&config, tx_pin, 1);
+    sm_config_set_sideset_pins(&config, tx_pin);
+    // We only need TX, so get an 8-deep FIFO!
+    sm_config_set_fifo_join(&config, PIO_FIFO_JOIN_TX);
+    // SM transmits 1 bit per 8 execution cycles.
+    float div = (float)clock_get_hz(clk_sys) / (8 * SERIAL_USART_SPEED);
+    sm_config_set_clkdiv(&config, div);
+    pio_sm_init(pio, tx_state_machine, offset, &config);
+    pio_sm_set_enabled(pio, tx_state_machine, true);
+}
+
+static inline void pio_rx_init(pin_t rx_pin) {
+    uint offset = pio_add_program(pio, &uart_rx_program);
+
+#if defined(SERIAL_USART_FULL_DUPLEX)
+    uint pio_idx = pio_get_index(pio);
+    pio_sm_set_consecutive_pindirs(pio, rx_state_machine, rx_pin, 1, false);
+    // clang-format off
+    iomode_t rx_pin_mode = PAL_RP_PAD_IE |
+                           PAL_RP_PAD_SCHMITT |
+                           PAL_RP_PAD_PUE |
+                           (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1);
+    // clang-format on
+    palSetLineMode(rx_pin, rx_pin_mode);
+#endif
+
+    pio_sm_config config = pio_get_default_sm_config();
+    sm_config_set_wrap(&config, offset + UART_RX_WRAP_TARGET, offset + UART_RX_WRAP);
+    sm_config_set_in_pins(&config, rx_pin); // for WAIT, IN
+    sm_config_set_jmp_pin(&config, rx_pin); // for JMP
+    // Shift to right, autopush disabled
+    sm_config_set_in_shift(&config, true, false, 32);
+    // Deeper FIFO as we're not doing any TX
+    sm_config_set_fifo_join(&config, PIO_FIFO_JOIN_RX);
+    // SM transmits 1 bit per 8 execution cycles.
+    float div = (float)clock_get_hz(clk_sys) / (8 * SERIAL_USART_SPEED);
+    sm_config_set_clkdiv(&config, div);
+    pio_sm_init(pio, rx_state_machine, offset, &config);
+    pio_sm_set_enabled(pio, rx_state_machine, true);
+}
+
+static inline void pio_init(pin_t tx_pin, pin_t rx_pin) {
+    uint pio_idx = pio_get_index(pio);
+
+    /* Get PIOx peripheral out of reset state. */
+    hal_lld_peripheral_unreset(pio_idx == 0 ? RESETS_ALLREG_PIO0 : RESETS_ALLREG_PIO1);
+
+    tx_state_machine = pio_claim_unused_sm(pio, true);
+    if (tx_state_machine < 0) {
+        dprintln("ERROR: Failed to acquire state machine for serial transmission!");
+        return;
+    }
+    pio_tx_init(tx_pin);
+
+    rx_state_machine = pio_claim_unused_sm(pio, true);
+    if (rx_state_machine < 0) {
+        dprintln("ERROR: Failed to acquire state machine for serial reception!");
+        return;
+    }
+    pio_rx_init(rx_pin);
+
+    // Enable error flag IRQ source for rx state machine
+    pio_set_irq0_source_enabled(pio, pis_sm0_rx_fifo_not_empty + rx_state_machine, true);
+    pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + tx_state_machine, true);
+    pio_set_irq0_source_enabled(pio, pis_interrupt0, true);
+
+    // Enable PIO specific interrupt vector, as the pio implementation is timing
+    // critical we use the highest possible priority.
+#if defined(SERIAL_PIO_USE_PIO1)
+    nvicEnableVector(RP_PIO1_IRQ_0_NUMBER, CORTEX_MAX_KERNEL_PRIORITY);
+#else
+    nvicEnableVector(RP_PIO0_IRQ_0_NUMBER, CORTEX_MAX_KERNEL_PRIORITY);
+#endif
+
+    enter_rx_state();
+}
+
+/**
+ * @brief PIO driver specific initialization function for the master side.
+ */
+void serial_transport_driver_master_init(void) {
+#if defined(SERIAL_USART_FULL_DUPLEX)
+    pin_t tx_pin = SERIAL_USART_TX_PIN;
+    pin_t rx_pin = SERIAL_USART_RX_PIN;
+#else
+    pin_t tx_pin = SERIAL_USART_TX_PIN;
+    pin_t rx_pin = SERIAL_USART_TX_PIN;
+#endif
+
+#if defined(SERIAL_USART_PIN_SWAP)
+    pio_init(rx_pin, tx_pin);
+#else
+    pio_init(tx_pin, rx_pin);
+#endif
+}
+
+/**
+ * @brief PIO driver specific initialization function for the slave side.
+ */
+void serial_transport_driver_slave_init(void) {
+#if defined(SERIAL_USART_FULL_DUPLEX)
+    pin_t tx_pin = SERIAL_USART_TX_PIN;
+    pin_t rx_pin = SERIAL_USART_RX_PIN;
+#else
+    pin_t tx_pin = SERIAL_USART_TX_PIN;
+    pin_t rx_pin = SERIAL_USART_TX_PIN;
+#endif
+
+    pio_init(tx_pin, rx_pin);
+}
diff --git a/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c b/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c
new file mode 100644
index 0000000000..bc34eded14
--- /dev/null
+++ b/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c
@@ -0,0 +1,189 @@
+// Copyright 2022 Stefan Kerkmann
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include "quantum.h"
+#include "ws2812.h"
+#include "hardware/pio.h"
+#include "hardware/clocks.h"
+
+#if !defined(MCU_RP)
+#    error PIO Driver is only available for Raspberry Pi 2040 MCUs!
+#endif
+
+#if defined(WS2812_PIO_USE_PIO1)
+static const PIO pio = pio1;
+#else
+static const PIO pio = pio0;
+#endif
+
+#if !defined(RP_DMA_PRIORITY_WS2812)
+#    define RP_DMA_PRIORITY_WS2812 12
+#endif
+
+static int state_machine = -1;
+
+#define WS2812_WRAP_TARGET 0
+#define WS2812_WRAP 3
+
+#define WS2812_T1 2
+#define WS2812_T2 5
+#define WS2812_T3 3
+
+#if defined(WS2812_EXTERNAL_PULLUP)
+
+#    pragma message "The GPIOs of the RP2040 are NOT 5V tolerant! Make sure to NOT apply any voltage over 3.3V to the RGB data pin."
+
+// clang-format off
+static const uint16_t ws2812_program_instructions[] = {
+            //     .wrap_target
+    0x7221, //  0: out    x, 1            side 1 [2] 
+    0x0123, //  1: jmp    !x, 3           side 0 [1] 
+    0x0400, //  2: jmp    0               side 0 [4] 
+    0xb442, //  3: nop                    side 1 [4] 
+            //     .wrap
+};
+
+#else
+
+static const uint16_t ws2812_program_instructions[] = {
+             //     .wrap_target
+    0x6221,  //  0: out    x, 1            side 0 [2]
+    0x1123,  //  1: jmp    !x, 3           side 1 [1]
+    0x1400,  //  2: jmp    0               side 1 [4]
+    0xa442,  //  3: nop                    side 0 [4]
+             //     .wrap
+};
+// clang-format on
+#endif
+
+static const pio_program_t ws2812_program = {
+    .instructions = ws2812_program_instructions,
+    .length       = 4,
+    .origin       = -1,
+};
+
+static uint32_t                WS2812_BUFFER[RGBLED_NUM];
+static const rp_dma_channel_t* WS2812_DMA_CHANNEL;
+
+bool ws2812_init(void) {
+    uint pio_idx = pio_get_index(pio);
+    /* Get PIOx peripheral out of reset state. */
+    hal_lld_peripheral_unreset(pio_idx == 0 ? RESETS_ALLREG_PIO0 : RESETS_ALLREG_PIO1);
+
+    // clang-format off
+    iomode_t rgb_pin_mode = PAL_RP_PAD_SLEWFAST |
+                            PAL_RP_GPIO_OE |
+                            (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1);
+    // clang-format on
+
+    palSetLineMode(RGB_DI_PIN, rgb_pin_mode);
+
+    state_machine = pio_claim_unused_sm(pio, true);
+    if (state_machine < 0) {
+        dprintln("ERROR: Failed to acquire state machine for WS2812 output!");
+        return false;
+    }
+
+    uint offset = pio_add_program(pio, &ws2812_program);
+
+    pio_sm_set_consecutive_pindirs(pio, state_machine, RGB_DI_PIN, 1, true);
+
+    pio_sm_config config = pio_get_default_sm_config();
+    sm_config_set_wrap(&config, offset + WS2812_WRAP_TARGET, offset + WS2812_WRAP);
+    sm_config_set_sideset_pins(&config, RGB_DI_PIN);
+    sm_config_set_fifo_join(&config, PIO_FIFO_JOIN_TX);
+
+#if defined(WS2812_EXTERNAL_PULLUP)
+    /* Instruct side-set to change the pin-directions instead of outputting
+     * a logic level. We generate our levels the following way:
+     *
+     * 1: Set RGB data pin to high impedance input and let the pull-up drive the
+     * signal high.
+     *
+     * 0: Set RGB data pin to low impedance output and drive the pin low.
+     */
+    sm_config_set_sideset(&config, 1, false, true);
+#else
+    sm_config_set_sideset(&config, 1, false, false);
+#endif
+
+#if defined(RGBW)
+    sm_config_set_out_shift(&config, false, true, 32);
+#else
+    sm_config_set_out_shift(&config, false, true, 24);
+#endif
+
+    int   cycles_per_bit = WS2812_T1 + WS2812_T2 + WS2812_T3;
+    float div            = clock_get_hz(clk_sys) / (800.0f * KHZ * cycles_per_bit);
+    sm_config_set_clkdiv(&config, div);
+
+    pio_sm_init(pio, state_machine, offset, &config);
+    pio_sm_set_enabled(pio, state_machine, true);
+
+    WS2812_DMA_CHANNEL = dmaChannelAlloc(RP_DMA_CHANNEL_ID_ANY, RP_DMA_PRIORITY_WS2812, NULL, NULL);
+
+    // clang-format off
+    uint32_t mode = DMA_CTRL_TRIG_INCR_READ |
+                    DMA_CTRL_TRIG_DATA_SIZE_WORD |
+                    DMA_CTRL_TRIG_IRQ_QUIET |
+                    DMA_CTRL_TRIG_TREQ_SEL(pio_idx == 0 ? state_machine : state_machine + 8);
+    // clang-format on
+
+    dmaChannelSetModeX(WS2812_DMA_CHANNEL, mode);
+    dmaChannelSetDestinationX(WS2812_DMA_CHANNEL, (uint32_t)&pio->txf[state_machine]);
+    return true;
+}
+
+/**
+ * @brief Convert RGBW value into WS2812 compatible 32-bit data word.
+ */
+__always_inline static uint32_t rgbw8888_to_u32(uint8_t red, uint8_t green, uint8_t blue, uint8_t white) {
+#if (WS2812_BYTE_ORDER == WS2812_BYTE_ORDER_GRB)
+    return ((uint32_t)green << 24) | ((uint32_t)red << 16) | ((uint32_t)blue << 8) | ((uint32_t)white);
+#elif (WS2812_BYTE_ORDER == WS2812_BYTE_ORDER_RGB)
+    return ((uint32_t)red << 24) | ((uint32_t)green << 16) | ((uint32_t)blue << 8) | ((uint32_t)white);
+#elif (WS2812_BYTE_ORDER == WS2812_BYTE_ORDER_BGR)
+    return ((uint32_t)blue << 24) | ((uint32_t)green << 16) | ((uint32_t)red << 8) | ((uint32_t)white);
+#endif
+}
+
+static inline void sync_ws2812_transfer(void) {
+    if (unlikely(dmaChannelIsBusyX(WS2812_DMA_CHANNEL) || !pio_sm_is_tx_fifo_empty(pio, state_machine))) {
+        fast_timer_t start = timer_read_fast();
+        do {
+            // Abort the synchronization if we have to wait longer than the total
+            // count of LEDs in millisecounds. This is safely much longer than it
+            // would take to push all the data out.
+            if (unlikely(timer_elapsed_fast(start) > RGBLED_NUM)) {
+                dprintln("ERROR: WS2812 DMA transfer has stalled, aborting!");
+                dmaChannelDisableX(WS2812_DMA_CHANNEL);
+                return;
+            }
+
+        } while (dmaChannelIsBusyX(WS2812_DMA_CHANNEL) || !pio_sm_is_tx_fifo_empty(pio, state_machine));
+        // We wait for the WS2812 chain to reset after all data has been pushed
+        // out.
+        wait_us(WS2812_TRST_US);
+    }
+}
+
+void ws2812_setleds(LED_TYPE* ledarray, uint16_t leds) {
+    static bool is_initialized = false;
+    if (unlikely(!is_initialized)) {
+        is_initialized = ws2812_init();
+    }
+
+    sync_ws2812_transfer();
+
+    for (int i = 0; i < leds; i++) {
+#if defined(RGBW)
+        WS2812_BUFFER[i] = rgbw8888_to_u32(ledarray[i].r, ledarray[i].g, ledarray[i].b, ledarray[i].w);
+#else
+        WS2812_BUFFER[i] = rgbw8888_to_u32(ledarray[i].r, ledarray[i].g, ledarray[i].b, 0);
+#endif
+    }
+
+    dmaChannelSetSourceX(WS2812_DMA_CHANNEL, (uint32_t)WS2812_BUFFER);
+    dmaChannelSetCounterX(WS2812_DMA_CHANNEL, leds);
+    dmaChannelEnableX(WS2812_DMA_CHANNEL);
+}
diff --git a/platforms/chibios/drivers/wear_leveling/wear_leveling_efl.c b/platforms/chibios/drivers/wear_leveling/wear_leveling_efl.c
new file mode 100644
index 0000000000..cdd1e26a7d
--- /dev/null
+++ b/platforms/chibios/drivers/wear_leveling/wear_leveling_efl.c
@@ -0,0 +1,143 @@
+// Copyright 2022 Nick Brassel (@tzarc)
+// SPDX-License-Identifier: GPL-2.0-or-later
+#include <stdbool.h>
+#include <hal.h>
+#include "timer.h"
+#include "wear_leveling.h"
+#include "wear_leveling_internal.h"
+
+static flash_offset_t base_offset = UINT32_MAX;
+
+#if defined(WEAR_LEVELING_EFL_FIRST_SECTOR)
+static flash_sector_t first_sector = WEAR_LEVELING_EFL_FIRST_SECTOR;
+#else  // defined(WEAR_LEVELING_EFL_FIRST_SECTOR)
+static flash_sector_t first_sector = UINT16_MAX;
+#endif // defined(WEAR_LEVELING_EFL_FIRST_SECTOR)
+
+static flash_sector_t sector_count = UINT16_MAX;
+static BaseFlash *    flash;
+
+#ifndef WEAR_LEVELING_EFL_FIRST_SECTOR
+// "Automatic" detection of the flash size -- ideally ChibiOS would have this already, but alas, it doesn't.
+static inline uint32_t detect_flash_size(void) {
+#    if defined(WEAR_LEVELING_EFL_FLASH_SIZE)
+    return WEAR_LEVELING_EFL_FLASH_SIZE;
+#    elif defined(FLASH_BANK_SIZE)
+    return FLASH_BANK_SIZE;
+#    elif defined(FLASH_SIZE)
+    return FLASH_SIZE;
+#    elif defined(FLASHSIZE_BASE)
+#        if defined(QMK_MCU_SERIES_STM32F0XX) || defined(QMK_MCU_SERIES_STM32F1XX) || defined(QMK_MCU_SERIES_STM32F3XX) || defined(QMK_MCU_SERIES_STM32F4XX) || defined(QMK_MCU_SERIES_STM32G4XX) || defined(QMK_MCU_SERIES_STM32L0XX) || defined(QMK_MCU_SERIES_STM32L4XX) || defined(QMK_MCU_SERIES_GD32VF103)
+    return ((*(uint32_t *)FLASHSIZE_BASE) & 0xFFFFU) << 10U; // this register has the flash size in kB, so we convert it to bytes
+#        elif defined(QMK_MCU_SERIES_STM32L1XX)
+#            error This MCU family has an uncommon flash size register definition and has not been implemented. Perhaps try using the true EEPROM on the MCU instead?
+#        endif
+#    else
+#        error Unknown flash size definition.
+    return 0;
+#    endif
+}
+#endif // WEAR_LEVELING_EFL_FIRST_SECTOR
+
+bool backing_store_init(void) {
+    bs_dprintf("Init\n");
+    flash = (BaseFlash *)&EFLD1;
+
+    // Need to re-lock the EFL, as if we've just had the bootloader executing it'll already be unlocked.
+    backing_store_lock();
+
+    const flash_descriptor_t *desc    = flashGetDescriptor(flash);
+    uint32_t                  counter = 0;
+
+#if defined(WEAR_LEVELING_EFL_FIRST_SECTOR)
+
+    // Work out how many sectors we want to use, working forwards from the first sector specified
+    for (flash_sector_t i = 0; i < desc->sectors_count - first_sector; ++i) {
+        counter += flashGetSectorSize(flash, first_sector + i);
+        if (counter >= (WEAR_LEVELING_BACKING_SIZE)) {
+            sector_count = i + 1;
+            base_offset  = flashGetSectorOffset(flash, first_sector);
+            break;
+        }
+    }
+    if (sector_count == UINT16_MAX || base_offset >= flash_size) {
+        // We didn't get the required number of sectors. Can't do anything here. Fault.
+        chSysHalt("Invalid sector count intended to be used with wear_leveling");
+    }
+
+#else // defined(WEAR_LEVELING_EFL_FIRST_SECTOR)
+
+    // Work out how many sectors we want to use, working backwards from the end of the flash
+    uint32_t       flash_size  = detect_flash_size();
+    flash_sector_t last_sector = desc->sectors_count;
+    for (flash_sector_t i = 0; i < desc->sectors_count; ++i) {
+        first_sector = desc->sectors_count - i - 1;
+        if (flashGetSectorOffset(flash, first_sector) >= flash_size) {
+            last_sector = first_sector;
+            continue;
+        }
+        counter += flashGetSectorSize(flash, first_sector);
+        if (counter >= (WEAR_LEVELING_BACKING_SIZE)) {
+            sector_count = last_sector - first_sector;
+            base_offset  = flashGetSectorOffset(flash, first_sector);
+            break;
+        }
+    }
+
+#endif // defined(WEAR_LEVELING_EFL_FIRST_SECTOR)
+
+    return true;
+}
+
+bool backing_store_unlock(void) {
+    bs_dprintf("Unlock\n");
+    return eflStart(&EFLD1, NULL) == HAL_RET_SUCCESS;
+}
+
+bool backing_store_erase(void) {
+#ifdef WEAR_LEVELING_DEBUG_OUTPUT
+    uint32_t start = timer_read32();
+#endif
+
+    bool          ret = true;
+    flash_error_t status;
+    for (int i = 0; i < sector_count; ++i) {
+        // Kick off the sector erase
+        status = flashStartEraseSector(flash, first_sector + i);
+        if (status != FLASH_NO_ERROR && status != FLASH_BUSY_ERASING) {
+            ret = false;
+        }
+
+        // Wait for the erase to complete
+        status = flashWaitErase(flash);
+        if (status != FLASH_NO_ERROR && status != FLASH_BUSY_ERASING) {
+            ret = false;
+        }
+    }
+
+    bs_dprintf("Backing store erase took %ldms to complete\n", ((long)(timer_read32() - start)));
+    return ret;
+}
+
+bool backing_store_write(uint32_t address, backing_store_int_t value) {
+    uint32_t offset = (base_offset + address);
+    bs_dprintf("Write ");
+    wl_dump(offset, &value, sizeof(value));
+    value = ~value;
+    return flashProgram(flash, offset, sizeof(value), (const uint8_t *)&value) == FLASH_NO_ERROR;
+}
+
+bool backing_store_lock(void) {
+    bs_dprintf("Lock  \n");
+    eflStop(&EFLD1);
+    return true;
+}
+
+bool backing_store_read(uint32_t address, backing_store_int_t *value) {
+    uint32_t             offset = (base_offset + address);
+    backing_store_int_t *loc    = (backing_store_int_t *)flashGetOffsetAddress(flash, offset);
+    *value                      = ~(*loc);
+    bs_dprintf("Read  ");
+    wl_dump(offset, value, sizeof(backing_store_int_t));
+    return true;
+}
diff --git a/platforms/chibios/drivers/wear_leveling/wear_leveling_efl_config.h b/platforms/chibios/drivers/wear_leveling/wear_leveling_efl_config.h
new file mode 100644
index 0000000000..244c87cb7f
--- /dev/null
+++ b/platforms/chibios/drivers/wear_leveling/wear_leveling_efl_config.h
@@ -0,0 +1,52 @@
+// Copyright 2022 Nick Brassel (@tzarc)
+// SPDX-License-Identifier: GPL-2.0-or-later
+#pragma once
+
+#ifndef __ASSEMBLER__
+#    include <hal.h>
+#endif
+
+// Work out how many bytes per write to internal flash
+#ifndef BACKING_STORE_WRITE_SIZE
+// These need to match EFL's XXXXXX_FLASH_LINE_SIZE, see associated code in `lib/chibios/os/hal/ports/**/hal_efl_lld.c`,
+// or associated `stm32_registry.h` for the MCU in question (or equivalent for the family).
+#    if defined(QMK_MCU_SERIES_GD32VF103)
+#        define BACKING_STORE_WRITE_SIZE 2 // from hal_efl_lld.c
+#    elif defined(QMK_MCU_FAMILY_NUC123)
+#        define BACKING_STORE_WRITE_SIZE 4 // from hal_efl_lld.c
+#    elif defined(QMK_MCU_FAMILY_WB32)
+#        define BACKING_STORE_WRITE_SIZE 8 // from hal_efl_lld.c
+#    elif defined(QMK_MCU_FAMILY_STM32)
+#        if defined(STM32_FLASH_LINE_SIZE) // from some family's stm32_registry.h file
+#            define BACKING_STORE_WRITE_SIZE (STM32_FLASH_LINE_SIZE)
+#        else
+#            if defined(QMK_MCU_SERIES_STM32F1XX)
+#                define BACKING_STORE_WRITE_SIZE 2 // from hal_efl_lld.c
+#            elif defined(QMK_MCU_SERIES_STM32F3XX)
+#                define BACKING_STORE_WRITE_SIZE 2 // from hal_efl_lld.c
+#            elif defined(QMK_MCU_SERIES_STM32F4XX)
+#                define BACKING_STORE_WRITE_SIZE (1 << STM32_FLASH_PSIZE) // from hal_efl_lld.c
+#            elif defined(QMK_MCU_SERIES_STM32L4XX)
+#                define BACKING_STORE_WRITE_SIZE 8 // from hal_efl_lld.c
+#            elif defined(QMK_MCU_SERIES_STM32G0XX)
+#                define BACKING_STORE_WRITE_SIZE 8 // from hal_efl_lld.c
+#            elif defined(QMK_MCU_SERIES_STM32G4XX)
+#                define BACKING_STORE_WRITE_SIZE 8 // from hal_efl_lld.c
+#            else
+#                error "ChibiOS hasn't defined STM32_FLASH_LINE_SIZE, and could not automatically determine BACKING_STORE_WRITE_SIZE" // normally defined in stm32_registry.h, should be set by STM32_FLASH_LINE_SIZE
+#            endif
+#        endif
+#    else
+#        error "Could not automatically determine BACKING_STORE_WRITE_SIZE"
+#    endif
+#endif
+
+// 2kB backing space allocated
+#ifndef WEAR_LEVELING_BACKING_SIZE
+#    define WEAR_LEVELING_BACKING_SIZE 2048
+#endif // WEAR_LEVELING_BACKING_SIZE
+
+// 1kB logical EEPROM
+#ifndef WEAR_LEVELING_LOGICAL_SIZE
+#    define WEAR_LEVELING_LOGICAL_SIZE 1024
+#endif // WEAR_LEVELING_LOGICAL_SIZE
diff --git a/platforms/chibios/drivers/wear_leveling/wear_leveling_legacy.c b/platforms/chibios/drivers/wear_leveling/wear_leveling_legacy.c
new file mode 100644
index 0000000000..87126c4467
--- /dev/null
+++ b/platforms/chibios/drivers/wear_leveling/wear_leveling_legacy.c
@@ -0,0 +1,59 @@
+// Copyright 2022 Nick Brassel (@tzarc)
+// SPDX-License-Identifier: GPL-2.0-or-later
+#include <stdbool.h>
+#include <hal.h>
+#include "timer.h"
+#include "wear_leveling.h"
+#include "wear_leveling_internal.h"
+#include "flash_stm32.h"
+
+bool backing_store_init(void) {
+    bs_dprintf("Init\n");
+    return true;
+}
+
+bool backing_store_unlock(void) {
+    bs_dprintf("Unlock\n");
+    FLASH_Unlock();
+    return true;
+}
+
+bool backing_store_erase(void) {
+#ifdef WEAR_LEVELING_DEBUG_OUTPUT
+    uint32_t start = timer_read32();
+#endif
+
+    bool         ret = true;
+    FLASH_Status status;
+    for (int i = 0; i < (WEAR_LEVELING_LEGACY_EMULATION_PAGE_COUNT); ++i) {
+        status = FLASH_ErasePage(WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS + (i * (WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE)));
+        if (status != FLASH_COMPLETE) {
+            ret = false;
+        }
+    }
+
+    bs_dprintf("Backing store erase took %ldms to complete\n", ((long)(timer_read32() - start)));
+    return ret;
+}
+
+bool backing_store_write(uint32_t address, backing_store_int_t value) {
+    uint32_t offset = ((WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS) + address);
+    bs_dprintf("Write ");
+    wl_dump(offset, &value, sizeof(backing_store_int_t));
+    return FLASH_ProgramHalfWord(offset, ~value) == FLASH_COMPLETE;
+}
+
+bool backing_store_lock(void) {
+    bs_dprintf("Lock  \n");
+    FLASH_Lock();
+    return true;
+}
+
+bool backing_store_read(uint32_t address, backing_store_int_t* value) {
+    uint32_t             offset = ((WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS) + address);
+    backing_store_int_t* loc    = (backing_store_int_t*)offset;
+    *value                      = ~(*loc);
+    bs_dprintf("Read  ");
+    wl_dump(offset, loc, sizeof(backing_store_int_t));
+    return true;
+}
diff --git a/platforms/chibios/drivers/wear_leveling/wear_leveling_legacy_config.h b/platforms/chibios/drivers/wear_leveling/wear_leveling_legacy_config.h
new file mode 100644
index 0000000000..e64cab87d1
--- /dev/null
+++ b/platforms/chibios/drivers/wear_leveling/wear_leveling_legacy_config.h
@@ -0,0 +1,67 @@
+// Copyright 2022 Nick Brassel (@tzarc)
+// SPDX-License-Identifier: GPL-2.0-or-later
+#pragma once
+
+// Work out the page size to use
+#ifndef WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE
+#    if defined(QMK_MCU_STM32F042)
+#        define WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE 1024
+#    elif defined(QMK_MCU_STM32F070) || defined(QMK_MCU_STM32F072)
+#        define WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE 2048
+#    elif defined(QMK_MCU_STM32F401) || defined(QMK_MCU_STM32F411)
+#        define WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE 16384
+#    endif
+#endif
+
+// Work out how much flash space we have
+#ifndef WEAR_LEVELING_LEGACY_EMULATION_FLASH_SIZE
+#    define WEAR_LEVELING_LEGACY_EMULATION_FLASH_SIZE ((*(uint32_t *)FLASHSIZE_BASE) & 0xFFFFU) // in kB
+#endif
+
+// The base location of program memory
+#ifndef WEAR_LEVELING_LEGACY_EMULATION_FLASH_BASE
+#    define WEAR_LEVELING_LEGACY_EMULATION_FLASH_BASE 0x08000000
+#endif
+
+// The number of pages to use
+#ifndef WEAR_LEVELING_LEGACY_EMULATION_PAGE_COUNT
+#    if defined(QMK_MCU_STM32F042)
+#        define WEAR_LEVELING_LEGACY_EMULATION_PAGE_COUNT 2
+#    elif defined(QMK_MCU_STM32F070) || defined(QMK_MCU_STM32F072)
+#        define WEAR_LEVELING_LEGACY_EMULATION_PAGE_COUNT 1
+#    elif defined(QMK_MCU_STM32F401) || defined(QMK_MCU_STM32F411)
+#        define WEAR_LEVELING_LEGACY_EMULATION_PAGE_COUNT 1
+#    endif
+#endif
+
+// The origin of the emulated eeprom
+#ifndef WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS
+#    if defined(QMK_MCU_STM32F042) || defined(QMK_MCU_STM32F070) || defined(QMK_MCU_STM32F072)
+#        define WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS ((uintptr_t)(WEAR_LEVELING_LEGACY_EMULATION_FLASH_BASE) + WEAR_LEVELING_LEGACY_EMULATION_FLASH_SIZE * 1024 - (WEAR_LEVELING_LEGACY_EMULATION_PAGE_COUNT * WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE))
+#    elif defined(QMK_MCU_STM32F401) || defined(QMK_MCU_STM32F411)
+#        if defined(BOOTLOADER_STM32_DFU)
+#            define WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS (WEAR_LEVELING_LEGACY_EMULATION_FLASH_BASE + (1 * (WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE))) // +16k
+#        elif defined(BOOTLOADER_TINYUF2)
+#            define WEAR_LEVELING_LEGACY_EMULATION_BASE_PAGE_ADDRESS (WEAR_LEVELING_LEGACY_EMULATION_FLASH_BASE + (3 * (WEAR_LEVELING_LEGACY_EMULATION_PAGE_SIZE))) // +48k
+#        endif
+#    endif
+#endif
+
+// 2-byte writes
+#ifndef BACKING_STORE_WRITE_SIZE
+#    define BACKING_STORE_WRITE_SIZE 2
+#endif
+
+// The amount of space to use for the entire set of emulation
+#ifndef WEAR_LEVELING_BACKING_SIZE
+#    if defined(QMK_MCU_STM32F042) || defined(QMK_MCU_STM32F070) || defined(QMK_MCU_STM32F072)
+#        define WEAR_LEVELING_BACKING_SIZE 2048
+#    elif defined(QMK_MCU_STM32F401) || defined(QMK_MCU_STM32F411)
+#        define WEAR_LEVELING_BACKING_SIZE 16384
+#    endif
+#endif
+
+// The logical amount of eeprom available
+#ifndef WEAR_LEVELING_LOGICAL_SIZE
+#    define WEAR_LEVELING_LOGICAL_SIZE 1024
+#endif
diff --git a/platforms/chibios/drivers/wear_leveling/wear_leveling_rp2040_flash.c b/platforms/chibios/drivers/wear_leveling/wear_leveling_rp2040_flash.c
new file mode 100644
index 0000000000..640628e1e9
--- /dev/null
+++ b/platforms/chibios/drivers/wear_leveling/wear_leveling_rp2040_flash.c
@@ -0,0 +1,221 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ * Copyright (c) 2022 Nick Brassel (@tzarc)
+ * Copyright (c) 2022 Stefan Kerkmann (@KarlK90)
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/bootrom.h"
+#include "hardware/flash.h"
+#include "hardware/sync.h"
+#include "hardware/structs/ssi.h"
+#include "hardware/structs/ioqspi.h"
+
+#include <stdbool.h>
+#include "timer.h"
+#include "wear_leveling.h"
+#include "wear_leveling_internal.h"
+
+#ifndef WEAR_LEVELING_RP2040_FLASH_BULK_COUNT
+#    define WEAR_LEVELING_RP2040_FLASH_BULK_COUNT 64
+#endif // WEAR_LEVELING_RP2040_FLASH_BULK_COUNT
+
+#define FLASHCMD_PAGE_PROGRAM 0x02
+#define FLASHCMD_READ_STATUS 0x05
+#define FLASHCMD_WRITE_ENABLE 0x06
+
+extern uint8_t  BOOT2_ROM[256];
+static uint32_t BOOT2_ROM_RAM[64];
+
+static ssi_hw_t *const ssi = (ssi_hw_t *)XIP_SSI_BASE;
+
+// Sanity check
+check_hw_layout(ssi_hw_t, ssienr, SSI_SSIENR_OFFSET);
+check_hw_layout(ssi_hw_t, spi_ctrlr0, SSI_SPI_CTRLR0_OFFSET);
+
+static void __no_inline_not_in_flash_func(flash_enable_xip_via_boot2)(void) {
+    ((void (*)(void))BOOT2_ROM_RAM + 1)();
+}
+
+// Bitbanging the chip select using IO overrides, in case RAM-resident IRQs
+// are still running, and the FIFO bottoms out. (the bootrom does the same)
+static void __no_inline_not_in_flash_func(flash_cs_force)(bool high) {
+    uint32_t field_val = high ? IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_VALUE_HIGH : IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_VALUE_LOW;
+    hw_write_masked(&ioqspi_hw->io[1].ctrl, field_val << IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_LSB, IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_BITS);
+}
+
+// Also allow any unbounded loops to check whether the above abort condition
+// was asserted, and terminate early
+static int __no_inline_not_in_flash_func(flash_was_aborted)(void) {
+    return *(io_rw_32 *)(IO_QSPI_BASE + IO_QSPI_GPIO_QSPI_SD1_CTRL_OFFSET) & IO_QSPI_GPIO_QSPI_SD1_CTRL_INOVER_BITS;
+}
+
+// Put bytes from one buffer, and get bytes into another buffer.
+// These can be the same buffer.
+// If tx is NULL then send zeroes.
+// If rx is NULL then all read data will be dropped.
+//
+// If rx_skip is nonzero, this many bytes will first be consumed from the FIFO,
+// before reading a further count bytes into *rx.
+// E.g. if you have written a command+address just before calling this function.
+static void __no_inline_not_in_flash_func(flash_put_get)(const uint8_t *tx, uint8_t *rx, size_t count, size_t rx_skip) {
+    // Make sure there is never more data in flight than the depth of the RX
+    // FIFO. Otherwise, when we are interrupted for long periods, hardware
+    // will overflow the RX FIFO.
+    const uint max_in_flight = 16 - 2; // account for data internal to SSI
+    size_t     tx_count      = count;
+    size_t     rx_count      = count;
+    while (tx_count || rx_skip || rx_count) {
+        // NB order of reads, for pessimism rather than optimism
+        uint32_t tx_level      = ssi_hw->txflr;
+        uint32_t rx_level      = ssi_hw->rxflr;
+        bool     did_something = false; // Expect this to be folded into control flow, not register
+        if (tx_count && tx_level + rx_level < max_in_flight) {
+            ssi->dr0 = (uint32_t)(tx ? *tx++ : 0);
+            --tx_count;
+            did_something = true;
+        }
+        if (rx_level) {
+            uint8_t rxbyte = ssi->dr0;
+            did_something  = true;
+            if (rx_skip) {
+                --rx_skip;
+            } else {
+                if (rx) *rx++ = rxbyte;
+                --rx_count;
+            }
+        }
+        // APB load costs 4 cycles, so only do it on idle loops (our budget is
+        // 48 cyc/byte)
+        if (!did_something && __builtin_expect(flash_was_aborted(), 0)) break;
+    }
+    flash_cs_force(1);
+}
+
+// Convenience wrapper for above
+// (And it's hard for the debug host to get the tight timing between
+// cmd DR0 write and the remaining data)
+static void __no_inline_not_in_flash_func(_flash_do_cmd)(uint8_t cmd, const uint8_t *tx, uint8_t *rx, size_t count) {
+    flash_cs_force(0);
+    ssi->dr0 = cmd;
+    flash_put_get(tx, rx, count, 1);
+}
+
+// Timing of this one is critical, so do not expose the symbol to debugger etc
+static void __no_inline_not_in_flash_func(flash_put_cmd_addr)(uint8_t cmd, uint32_t addr) {
+    flash_cs_force(0);
+    addr |= cmd << 24;
+    for (int i = 0; i < 4; ++i) {
+        ssi->dr0 = addr >> 24;
+        addr <<= 8;
+    }
+}
+
+// Poll the flash status register until the busy bit (LSB) clears
+static void __no_inline_not_in_flash_func(flash_wait_ready)(void) {
+    uint8_t stat;
+    do {
+        _flash_do_cmd(FLASHCMD_READ_STATUS, NULL, &stat, 1);
+    } while (stat & 0x1 && !flash_was_aborted());
+}
+
+// Set the WEL bit (needed before any program/erase operation)
+static void __no_inline_not_in_flash_func(flash_enable_write)(void) {
+    _flash_do_cmd(FLASHCMD_WRITE_ENABLE, NULL, NULL, 0);
+}
+
+static void __no_inline_not_in_flash_func(pico_program_bulk)(uint32_t flash_address, backing_store_int_t *values, size_t item_count) {
+    rom_connect_internal_flash_fn connect_internal_flash = (rom_connect_internal_flash_fn)rom_func_lookup_inline(ROM_FUNC_CONNECT_INTERNAL_FLASH);
+    rom_flash_exit_xip_fn         flash_exit_xip         = (rom_flash_exit_xip_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_EXIT_XIP);
+    rom_flash_flush_cache_fn      flash_flush_cache      = (rom_flash_flush_cache_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_FLUSH_CACHE);
+    assert(connect_internal_flash && flash_exit_xip && flash_flush_cache);
+
+    static backing_store_int_t bulk_write_buffer[WEAR_LEVELING_RP2040_FLASH_BULK_COUNT];
+
+    while (item_count) {
+        size_t batch_size = MIN(item_count, WEAR_LEVELING_RP2040_FLASH_BULK_COUNT);
+        for (size_t i = 0; i < batch_size; i++, values++, item_count--) {
+            bulk_write_buffer[i] = ~(*values);
+        }
+        __compiler_memory_barrier();
+
+        connect_internal_flash();
+        flash_exit_xip();
+        flash_enable_write();
+
+        flash_put_cmd_addr(FLASHCMD_PAGE_PROGRAM, flash_address);
+        flash_put_get((uint8_t *)bulk_write_buffer, NULL, batch_size * sizeof(backing_store_int_t), 4);
+        flash_wait_ready();
+        flash_address += batch_size * sizeof(backing_store_int_t);
+
+        flash_flush_cache();
+        flash_enable_xip_via_boot2();
+    }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// QMK Wear-Leveling Backing Store implementation
+
+static int interrupts;
+
+bool backing_store_init(void) {
+    bs_dprintf("Init\n");
+    memcpy(BOOT2_ROM_RAM, BOOT2_ROM, sizeof(BOOT2_ROM));
+    __compiler_memory_barrier();
+    return true;
+}
+
+bool backing_store_unlock(void) {
+    bs_dprintf("Unlock\n");
+    return true;
+}
+
+bool backing_store_erase(void) {
+#ifdef WEAR_LEVELING_DEBUG_OUTPUT
+    uint32_t start = timer_read32();
+#endif
+
+    // Ensure the backing size can be cleanly subtracted from the flash size without alignment issues.
+    _Static_assert((WEAR_LEVELING_BACKING_SIZE) % (FLASH_SECTOR_SIZE) == 0, "Backing size must be a multiple of FLASH_SECTOR_SIZE");
+
+    interrupts = save_and_disable_interrupts();
+    flash_range_erase((WEAR_LEVELING_RP2040_FLASH_BASE), (WEAR_LEVELING_BACKING_SIZE));
+    restore_interrupts(interrupts);
+
+    bs_dprintf("Backing store erase took %ldms to complete\n", ((long)(timer_read32() - start)));
+    return true;
+}
+
+bool backing_store_write(uint32_t address, backing_store_int_t value) {
+    return backing_store_write_bulk(address, &value, 1);
+}
+
+bool backing_store_write_bulk(uint32_t address, backing_store_int_t *values, size_t item_count) {
+    uint32_t offset = (WEAR_LEVELING_RP2040_FLASH_BASE) + address;
+    bs_dprintf("Write ");
+    wl_dump(offset, values, sizeof(backing_store_int_t) * item_count);
+    interrupts = save_and_disable_interrupts();
+    pico_program_bulk(offset, values, item_count);
+    restore_interrupts(interrupts);
+    return true;
+}
+
+bool backing_store_lock(void) {
+    return true;
+}
+
+bool backing_store_read(uint32_t address, backing_store_int_t *value) {
+    return backing_store_read_bulk(address, value, 1);
+}
+
+bool backing_store_read_bulk(uint32_t address, backing_store_int_t *values, size_t item_count) {
+    uint32_t             offset = (WEAR_LEVELING_RP2040_FLASH_BASE) + address;
+    backing_store_int_t *loc    = (backing_store_int_t *)((XIP_BASE) + offset);
+    for (size_t i = 0; i < item_count; ++i) {
+        values[i] = ~loc[i];
+    }
+    bs_dprintf("Read  ");
+    wl_dump(offset, values, item_count * sizeof(backing_store_int_t));
+    return true;
+}
diff --git a/platforms/chibios/drivers/wear_leveling/wear_leveling_rp2040_flash_config.h b/platforms/chibios/drivers/wear_leveling/wear_leveling_rp2040_flash_config.h
new file mode 100644
index 0000000000..93a9aa0372
--- /dev/null
+++ b/platforms/chibios/drivers/wear_leveling/wear_leveling_rp2040_flash_config.h
@@ -0,0 +1,32 @@
+// Copyright 2022 Nick Brassel (@tzarc)
+// SPDX-License-Identifier: GPL-2.0-or-later
+#pragma once
+
+#ifndef __ASSEMBLER__
+#    include "hardware/flash.h"
+#endif
+
+// 2-byte writes
+#ifndef BACKING_STORE_WRITE_SIZE
+#    define BACKING_STORE_WRITE_SIZE 2
+#endif
+
+// 64kB backing space allocated
+#ifndef WEAR_LEVELING_BACKING_SIZE
+#    define WEAR_LEVELING_BACKING_SIZE 8192
+#endif // WEAR_LEVELING_BACKING_SIZE
+
+// 32kB logical EEPROM
+#ifndef WEAR_LEVELING_LOGICAL_SIZE
+#    define WEAR_LEVELING_LOGICAL_SIZE 4096
+#endif // WEAR_LEVELING_LOGICAL_SIZE
+
+// Define how much flash space we have (defaults to lib/pico-sdk/src/boards/include/boards/***)
+#ifndef WEAR_LEVELING_RP2040_FLASH_SIZE
+#    define WEAR_LEVELING_RP2040_FLASH_SIZE (PICO_FLASH_SIZE_BYTES)
+#endif
+
+// Define the location of emulated EEPROM
+#ifndef WEAR_LEVELING_RP2040_FLASH_BASE
+#    define WEAR_LEVELING_RP2040_FLASH_BASE ((WEAR_LEVELING_RP2040_FLASH_SIZE) - (WEAR_LEVELING_BACKING_SIZE))
+#endif
diff --git a/platforms/chibios/drivers/ws2812.c b/platforms/chibios/drivers/ws2812.c
index 1b3bb59842..55ac333b1e 100644
--- a/platforms/chibios/drivers/ws2812.c
+++ b/platforms/chibios/drivers/ws2812.c
@@ -6,7 +6,7 @@
 /* Adapted from https://github.com/bigjosh/SimpleNeoPixelDemo/ */
 
 #ifndef NOP_FUDGE
-#    if defined(STM32F0XX) || defined(STM32F1XX) || defined(GD32VF103) || defined(STM32F3XX) || defined(STM32F4XX) || defined(STM32L0XX)
+#    if defined(STM32F0XX) || defined(STM32F1XX) || defined(GD32VF103) || defined(STM32F3XX) || defined(STM32F4XX) || defined(STM32L0XX) || defined(WB32F3G71xx) || defined(WB32FQ95xx)
 #        define NOP_FUDGE 0.4
 #    else
 #        error("NOP_FUDGE configuration required")
diff --git a/platforms/chibios/drivers/ws2812_pwm.c b/platforms/chibios/drivers/ws2812_pwm.c
index 57187676d7..792de85ce9 100644
--- a/platforms/chibios/drivers/ws2812_pwm.c
+++ b/platforms/chibios/drivers/ws2812_pwm.c
@@ -29,6 +29,22 @@
 #    error "please consult your MCU's datasheet and specify in your config.h: #define WS2812_DMAMUX_ID STM32_DMAMUX1_TIM?_UP"
 #endif
 
+/* Summarize https://www.st.com/resource/en/application_note/an4013-stm32-crossseries-timer-overview-stmicroelectronics.pdf to
+ * figure out if we are using a 32bit timer. This is needed to setup the DMA controller correctly.
+ * Ignore STM32H7XX and STM32U5XX as they are not supported by ChibiOS.
+ */
+#if !defined(STM32F1XX) && !defined(STM32L0XX) && !defined(STM32L1XX)
+#    define WS2812_PWM_TIMER_32BIT_PWMD2 1
+#endif
+#if !defined(STM32F1XX)
+#    define WS2812_PWM_TIMER_32BIT_PWMD5 1
+#endif
+#define WS2812_CONCAT1(a, b) a##b
+#define WS2812_CONCAT(a, b) WS2812_CONCAT1(a, b)
+#if WS2812_CONCAT(WS2812_PWM_TIMER_32BIT_, WS2812_PWM_DRIVER)
+#    define WS2812_PWM_TIMER_32BIT
+#endif
+
 #ifndef WS2812_PWM_COMPLEMENTARY_OUTPUT
 #    define WS2812_PWM_OUTPUT_MODE PWM_OUTPUT_ACTIVE_HIGH
 #else
@@ -89,6 +105,9 @@
  * The duty cycle is calculated for a high period of 350 nS.
  */
 #define WS2812_DUTYCYCLE_0 (WS2812_PWM_FREQUENCY / (1000000000 / 350))
+#if (WS2812_DUTYCYCLE_0 > 255)
+#    error WS2812 PWM driver: High period for a 0 is more than a byte
+#endif
 
 /**
  * @brief   High period for a one, in ticks
@@ -105,6 +124,9 @@
  * This is in the middle of the specifications of the WS2812 and WS2812B.
  */
 #define WS2812_DUTYCYCLE_1 (WS2812_PWM_FREQUENCY / (1000000000 / 800))
+#if (WS2812_DUTYCYCLE_1 > 255)
+#    error WS2812 PWM driver: High period for a 1 is more than a byte
+#endif
 
 /* --- PRIVATE MACROS ------------------------------------------------------- */
 
@@ -247,13 +269,35 @@
 
 /* --- PRIVATE VARIABLES ---------------------------------------------------- */
 
-static uint32_t ws2812_frame_buffer[WS2812_BIT_N + 1]; /**< Buffer for a frame */
+// STM32F2XX, STM32F4XX and STM32F7XX do NOT zero pad DMA transfers of unequal data width. Buffer width must match TIMx CCR.
+// For all other STM32 DMA transfer will automatically zero pad. We only need to set the right peripheral width.
+#if defined(STM32F2XX) || defined(STM32F4XX) || defined(STM32F7XX)
+#    if defined(WS2812_PWM_TIMER_32BIT)
+#        define WS2812_DMA_MEMORY_WIDTH STM32_DMA_CR_MSIZE_WORD
+#        define WS2812_DMA_PERIPHERAL_WIDTH STM32_DMA_CR_PSIZE_WORD
+typedef uint32_t ws2812_buffer_t;
+#    else
+#        define WS2812_DMA_MEMORY_WIDTH STM32_DMA_CR_MSIZE_HWORD
+#        define WS2812_DMA_PERIPHERAL_WIDTH STM32_DMA_CR_PSIZE_HWORD
+typedef uint16_t ws2812_buffer_t;
+#    endif
+#else
+#    define WS2812_DMA_MEMORY_WIDTH STM32_DMA_CR_MSIZE_BYTE
+#    if defined(WS2812_PWM_TIMER_32BIT)
+#        define WS2812_DMA_PERIPHERAL_WIDTH STM32_DMA_CR_PSIZE_WORD
+#    else
+#        define WS2812_DMA_PERIPHERAL_WIDTH STM32_DMA_CR_PSIZE_HWORD
+#    endif
+typedef uint8_t ws2812_buffer_t;
+#endif
+
+static ws2812_buffer_t ws2812_frame_buffer[WS2812_BIT_N + 1]; /**< Buffer for a frame */
 
 /* --- PUBLIC FUNCTIONS ----------------------------------------------------- */
 /*
  * Gedanke: Double-buffer type transactions: double buffer transfers using two memory pointers for
-the memory (while the DMA is reading/writing from/to a buffer, the application can
-write/read to/from the other buffer).
+ * the memory (while the DMA is reading/writing from/to a buffer, the application can
+ * write/read to/from the other buffer).
  */
 
 void ws2812_init(void) {
@@ -284,11 +328,18 @@ void ws2812_init(void) {
 
     // Configure DMA
     // dmaInit(); // Joe added this
+#if defined(WB32F3G71xx) || defined(WB32FQ95xx)
+    dmaStreamAlloc(WS2812_DMA_STREAM - WB32_DMA_STREAM(0), 10, NULL, NULL);
+    dmaStreamSetSource(WS2812_DMA_STREAM, ws2812_frame_buffer);
+    dmaStreamSetDestination(WS2812_DMA_STREAM, &(WS2812_PWM_DRIVER.tim->CCR[WS2812_PWM_CHANNEL - 1])); // Ziel ist der An-Zeit im Cap-Comp-Register
+    dmaStreamSetMode(WS2812_DMA_STREAM, WB32_DMA_CHCFG_HWHIF(WS2812_DMA_CHANNEL) | WB32_DMA_CHCFG_DIR_M2P | WB32_DMA_CHCFG_PSIZE_WORD | WB32_DMA_CHCFG_MSIZE_WORD | WB32_DMA_CHCFG_MINC | WB32_DMA_CHCFG_CIRC | WB32_DMA_CHCFG_TCIE | WB32_DMA_CHCFG_PL(3));
+#else
     dmaStreamAlloc(WS2812_DMA_STREAM - STM32_DMA_STREAM(0), 10, NULL, NULL);
     dmaStreamSetPeripheral(WS2812_DMA_STREAM, &(WS2812_PWM_DRIVER.tim->CCR[WS2812_PWM_CHANNEL - 1])); // Ziel ist der An-Zeit im Cap-Comp-Register
     dmaStreamSetMemory0(WS2812_DMA_STREAM, ws2812_frame_buffer);
+    dmaStreamSetMode(WS2812_DMA_STREAM, STM32_DMA_CR_CHSEL(WS2812_DMA_CHANNEL) | STM32_DMA_CR_DIR_M2P | WS2812_DMA_PERIPHERAL_WIDTH | WS2812_DMA_MEMORY_WIDTH | STM32_DMA_CR_MINC | STM32_DMA_CR_CIRC | STM32_DMA_CR_PL(3));
+#endif
     dmaStreamSetTransactionSize(WS2812_DMA_STREAM, WS2812_BIT_N);
-    dmaStreamSetMode(WS2812_DMA_STREAM, STM32_DMA_CR_CHSEL(WS2812_DMA_CHANNEL) | STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | STM32_DMA_CR_MINC | STM32_DMA_CR_CIRC | STM32_DMA_CR_PL(3));
     // M2P: Memory 2 Periph; PL: Priority Level
 
 #if (STM32_DMA_SUPPORTS_DMAMUX == TRUE)
diff --git a/platforms/chibios/drivers/ws2812_spi.c b/platforms/chibios/drivers/ws2812_spi.c
index 01d8148875..a73eb69720 100644
--- a/platforms/chibios/drivers/ws2812_spi.c
+++ b/platforms/chibios/drivers/ws2812_spi.c
@@ -16,6 +16,10 @@
 #    define WS2812_SPI_SCK_PAL_MODE 5
 #endif
 
+#ifndef WS2812_SPI_DIVISOR
+#    define WS2812_SPI_DIVISOR 16
+#endif
+
 // Push Pull or Open Drain Configuration
 // Default Push Pull
 #ifndef WS2812_EXTERNAL_PULLUP
@@ -42,7 +46,7 @@
 #    define WS2812_SPI_DIVISOR_CR1_BR_X (SPI_CR1_BR_0)
 #elif WS2812_SPI_DIVISOR == 8
 #    define WS2812_SPI_DIVISOR_CR1_BR_X (SPI_CR1_BR_1)
-#elif WS2812_SPI_DIVISOR == 16 // same as default
+#elif WS2812_SPI_DIVISOR == 16 // default
 #    define WS2812_SPI_DIVISOR_CR1_BR_X (SPI_CR1_BR_1 | SPI_CR1_BR_0)
 #elif WS2812_SPI_DIVISOR == 32
 #    define WS2812_SPI_DIVISOR_CR1_BR_X (SPI_CR1_BR_2)
@@ -53,7 +57,7 @@
 #elif WS2812_SPI_DIVISOR == 256
 #    define WS2812_SPI_DIVISOR_CR1_BR_X (SPI_CR1_BR_2 | SPI_CR1_BR_1 | SPI_CR1_BR_0)
 #else
-#    define WS2812_SPI_DIVISOR_CR1_BR_X (SPI_CR1_BR_1 | SPI_CR1_BR_0) // default
+#    error "Configured WS2812_SPI_DIVISOR value is not supported at this time."
 #endif
 
 // Use SPI circular buffer
@@ -148,8 +152,14 @@ void ws2812_init(void) {
         NULL, // end_cb
         PAL_PORT(RGB_DI_PIN),
         PAL_PAD(RGB_DI_PIN),
+#    if defined(WB32F3G71xx) || defined(WB32FQ95xx)
+        0,
+        0,
+        WS2812_SPI_DIVISOR
+#    else
         WS2812_SPI_DIVISOR_CR1_BR_X,
         0
+#    endif
 #else
     // HAL_SPI_V2
 #    if SPI_SUPPORTS_CIRCULAR == TRUE