9 files changed, 201 insertions, 115 deletions

diff --git a/drivers/avr/serial.c b/drivers/avr/serial.c
index c27cbfdd0..526a0946b 100644
--- a/drivers/avr/serial.c
+++ b/drivers/avr/serial.c
@@ -20,50 +20,111 @@
 
 #ifdef SOFT_SERIAL_PIN
 
-#    ifdef __AVR_ATmega32U4__
-// if using ATmega32U4 I2C, can not use PD0 and PD1 in soft serial.
-#        ifdef USE_AVR_I2C
-#            if SOFT_SERIAL_PIN == D0 || SOFT_SERIAL_PIN == D1
-#                error Using ATmega32U4 I2C, so can not use PD0, PD1
-#            endif
+#    if !(defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1287__) || defined(__AVR_ATmega16U2__) || defined(__AVR_ATmega32U2__) || defined(__AVR_ATmega16U4__) || defined(__AVR_ATmega32U4__))
+#        error serial.c is not supported for the currently selected MCU
+#    endif
+// if using ATmega32U4/2, AT90USBxxx I2C, can not use PD0 and PD1 in soft serial.
+#    if defined(__AVR_ATmega16U4__) || defined(__AVR_ATmega32U4__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1287__)
+#        if defined(USE_AVR_I2C) && (SOFT_SERIAL_PIN == D0 || SOFT_SERIAL_PIN == D1)
+#            error Using I2C, so can not use PD0, PD1
 #        endif
+#    endif
+// PD0..PD3, common config
+#    if SOFT_SERIAL_PIN == D0
+#        define EIMSK_BIT _BV(INT0)
+#        define EICRx_BIT (~(_BV(ISC00) | _BV(ISC01)))
+#        define SERIAL_PIN_INTERRUPT INT0_vect
+#        define EICRx EICRA
+#    elif SOFT_SERIAL_PIN == D1
+#        define EIMSK_BIT _BV(INT1)
+#        define EICRx_BIT (~(_BV(ISC10) | _BV(ISC11)))
+#        define SERIAL_PIN_INTERRUPT INT1_vect
+#        define EICRx EICRA
+#    elif SOFT_SERIAL_PIN == D2
+#        define EIMSK_BIT _BV(INT2)
+#        define EICRx_BIT (~(_BV(ISC20) | _BV(ISC21)))
+#        define SERIAL_PIN_INTERRUPT INT2_vect
+#        define EICRx EICRA
+#    elif SOFT_SERIAL_PIN == D3
+#        define EIMSK_BIT _BV(INT3)
+#        define EICRx_BIT (~(_BV(ISC30) | _BV(ISC31)))
+#        define SERIAL_PIN_INTERRUPT INT3_vect
+#        define EICRx EICRA
+#    endif
 
-#        define setPinInputHigh(pin) (DDRx_ADDRESS(pin) &= ~_BV((pin)&0xF), PORTx_ADDRESS(pin) |= _BV((pin)&0xF))
-#        define setPinOutput(pin) (DDRx_ADDRESS(pin) |= _BV((pin)&0xF))
-#        define writePinHigh(pin) (PORTx_ADDRESS(pin) |= _BV((pin)&0xF))
-#        define writePinLow(pin) (PORTx_ADDRESS(pin) &= ~_BV((pin)&0xF))
-#        define readPin(pin) ((bool)(PINx_ADDRESS(pin) & _BV((pin)&0xF)))
-
-#        if SOFT_SERIAL_PIN >= D0 && SOFT_SERIAL_PIN <= D3
-#            if SOFT_SERIAL_PIN == D0
-#                define EIMSK_BIT _BV(INT0)
-#                define EICRx_BIT (~(_BV(ISC00) | _BV(ISC01)))
-#                define SERIAL_PIN_INTERRUPT INT0_vect
-#            elif SOFT_SERIAL_PIN == D1
-#                define EIMSK_BIT _BV(INT1)
-#                define EICRx_BIT (~(_BV(ISC10) | _BV(ISC11)))
-#                define SERIAL_PIN_INTERRUPT INT1_vect
-#            elif SOFT_SERIAL_PIN == D2
-#                define EIMSK_BIT _BV(INT2)
-#                define EICRx_BIT (~(_BV(ISC20) | _BV(ISC21)))
-#                define SERIAL_PIN_INTERRUPT INT2_vect
-#            elif SOFT_SERIAL_PIN == D3
-#                define EIMSK_BIT _BV(INT3)
-#                define EICRx_BIT (~(_BV(ISC30) | _BV(ISC31)))
-#                define SERIAL_PIN_INTERRUPT INT3_vect
-#            endif
+// ATmegaxxU2 specific config
+#    if defined(__AVR_ATmega16U2__) || defined(__AVR_ATmega32U2__)
+// PD4(INT5), PD6(INT6), PD7(INT7), PC7(INT4)
+#        if SOFT_SERIAL_PIN == D4
+#            define EIMSK_BIT _BV(INT5)
+#            define EICRx_BIT (~(_BV(ISC50) | _BV(ISC51)))
+#            define SERIAL_PIN_INTERRUPT INT5_vect
+#            define EICRx EICRB
+#        elif SOFT_SERIAL_PIN == D6
+#            define EIMSK_BIT _BV(INT6)
+#            define EICRx_BIT (~(_BV(ISC60) | _BV(ISC61)))
+#            define SERIAL_PIN_INTERRUPT INT6_vect
+#            define EICRx EICRB
+#        elif SOFT_SERIAL_PIN == D7
+#            define EIMSK_BIT _BV(INT7)
+#            define EICRx_BIT (~(_BV(ISC70) | _BV(ISC71)))
+#            define SERIAL_PIN_INTERRUPT INT7_vect
+#            define EICRx EICRB
+#        elif SOFT_SERIAL_PIN == C7
+#            define EIMSK_BIT _BV(INT4)
+#            define EICRx_BIT (~(_BV(ISC40) | _BV(ISC41)))
+#            define SERIAL_PIN_INTERRUPT INT4_vect
+#            define EICRx EICRB
+#        endif
+#    endif
+
+// ATmegaxxU4 specific config
+#    if defined(__AVR_ATmega16U4__) || defined(__AVR_ATmega32U4__)
+// PE6(INT6)
+#        if SOFT_SERIAL_PIN == E6
+#            define EIMSK_BIT _BV(INT6)
+#            define EICRx_BIT (~(_BV(ISC60) | _BV(ISC61)))
+#            define SERIAL_PIN_INTERRUPT INT6_vect
+#            define EICRx EICRB
+#        endif
+#    endif
+
+// AT90USBxxx specific config
+#    if defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1287__)
+// PE4..PE7(INT4..INT7)
+#        if SOFT_SERIAL_PIN == E4
+#            define EIMSK_BIT _BV(INT4)
+#            define EICRx_BIT (~(_BV(ISC40) | _BV(ISC41)))
+#            define SERIAL_PIN_INTERRUPT INT4_vect
+#            define EICRx EICRB
+#        elif SOFT_SERIAL_PIN == E5
+#            define EIMSK_BIT _BV(INT5)
+#            define EICRx_BIT (~(_BV(ISC50) | _BV(ISC51)))
+#            define SERIAL_PIN_INTERRUPT INT5_vect
+#            define EICRx EICRB
 #        elif SOFT_SERIAL_PIN == E6
 #            define EIMSK_BIT _BV(INT6)
 #            define EICRx_BIT (~(_BV(ISC60) | _BV(ISC61)))
 #            define SERIAL_PIN_INTERRUPT INT6_vect
-#        else
-#            error invalid SOFT_SERIAL_PIN value
+#            define EICRx EICRB
+#        elif SOFT_SERIAL_PIN == E7
+#            define EIMSK_BIT _BV(INT7)
+#            define EICRx_BIT (~(_BV(ISC70) | _BV(ISC71)))
+#            define SERIAL_PIN_INTERRUPT INT7_vect
+#            define EICRx EICRB
 #        endif
+#    endif
 
-#    else
-#        error serial.c now support ATmega32U4 only
+#    ifndef SERIAL_PIN_INTERRUPT
+#        error invalid SOFT_SERIAL_PIN value
 #    endif
 
+#    define setPinInputHigh(pin) (DDRx_ADDRESS(pin) &= ~_BV((pin)&0xF), PORTx_ADDRESS(pin) |= _BV((pin)&0xF))
+#    define setPinOutput(pin) (DDRx_ADDRESS(pin) |= _BV((pin)&0xF))
+#    define writePinHigh(pin) (PORTx_ADDRESS(pin) |= _BV((pin)&0xF))
+#    define writePinLow(pin) (PORTx_ADDRESS(pin) &= ~_BV((pin)&0xF))
+#    define readPin(pin) ((bool)(PINx_ADDRESS(pin) & _BV((pin)&0xF)))
+
 #    define ALWAYS_INLINE __attribute__((always_inline))
 #    define NO_INLINE __attribute__((noinline))
 #    define _delay_sub_us(x) __builtin_avr_delay_cycles(x)
@@ -210,15 +271,9 @@ void soft_serial_target_init(SSTD_t *sstd_table, int sstd_table_size) {
     Transaction_table_size = (uint8_t)sstd_table_size;
     serial_input_with_pullup();
 
-    // Enable INT0-INT3,INT6
+    // Enable INT0-INT7
     EIMSK |= EIMSK_BIT;
-#    if SOFT_SERIAL_PIN == E6
-    // Trigger on falling edge of INT6
-    EICRB &= EICRx_BIT;
-#    else
-    // Trigger on falling edge of INT0-INT3
-    EICRA &= EICRx_BIT;
-#    endif
+    EICRx &= EICRx_BIT;
 }
 
 // Used by the sender to synchronize timing with the reciver.
diff --git a/drivers/chibios/i2c_master.c b/drivers/chibios/i2c_master.c
index 4bd8e2af7..fc4bb2ab3 100644
--- a/drivers/chibios/i2c_master.c
+++ b/drivers/chibios/i2c_master.c
@@ -58,18 +58,23 @@ static i2c_status_t chibios_to_qmk(const msg_t* status) {
 }
 
 __attribute__((weak)) void i2c_init(void) {
-    // Try releasing special pins for a short time
-    palSetPadMode(I2C1_SCL_BANK, I2C1_SCL, PAL_MODE_INPUT);
-    palSetPadMode(I2C1_SDA_BANK, I2C1_SDA, PAL_MODE_INPUT);
+    static bool is_initialised = false;
+    if (!is_initialised) {
+        is_initialised = true;
 
-    chThdSleepMilliseconds(10);
+        // Try releasing special pins for a short time
+        palSetPadMode(I2C1_SCL_BANK, I2C1_SCL, PAL_MODE_INPUT);
+        palSetPadMode(I2C1_SDA_BANK, I2C1_SDA, PAL_MODE_INPUT);
+
+        chThdSleepMilliseconds(10);
 #if defined(USE_GPIOV1)
-    palSetPadMode(I2C1_SCL_BANK, I2C1_SCL, I2C1_SCL_PAL_MODE);
-    palSetPadMode(I2C1_SDA_BANK, I2C1_SDA, I2C1_SDA_PAL_MODE);
+        palSetPadMode(I2C1_SCL_BANK, I2C1_SCL, I2C1_SCL_PAL_MODE);
+        palSetPadMode(I2C1_SDA_BANK, I2C1_SDA, I2C1_SDA_PAL_MODE);
 #else
-    palSetPadMode(I2C1_SCL_BANK, I2C1_SCL, PAL_MODE_ALTERNATE(I2C1_SCL_PAL_MODE) | PAL_STM32_OTYPE_OPENDRAIN);
-    palSetPadMode(I2C1_SDA_BANK, I2C1_SDA, PAL_MODE_ALTERNATE(I2C1_SDA_PAL_MODE) | PAL_STM32_OTYPE_OPENDRAIN);
+        palSetPadMode(I2C1_SCL_BANK, I2C1_SCL, PAL_MODE_ALTERNATE(I2C1_SCL_PAL_MODE) | PAL_STM32_OTYPE_OPENDRAIN);
+        palSetPadMode(I2C1_SDA_BANK, I2C1_SDA, PAL_MODE_ALTERNATE(I2C1_SDA_PAL_MODE) | PAL_STM32_OTYPE_OPENDRAIN);
 #endif
+    }
 }
 
 i2c_status_t i2c_start(uint8_t address) {
diff --git a/drivers/chibios/spi_master.c b/drivers/chibios/spi_master.c
index 552ac663c..5aa60742e 100644
--- a/drivers/chibios/spi_master.c
+++ b/drivers/chibios/spi_master.c
@@ -22,21 +22,26 @@ static pin_t     currentSlavePin = NO_PIN;
 static SPIConfig spiConfig       = {false, NULL, 0, 0, 0, 0};
 
 __attribute__((weak)) void spi_init(void) {
-    // Try releasing special pins for a short time
-    palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_INPUT);
-    palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_INPUT);
-    palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_INPUT);
+    static bool is_initialised = false;
+    if (!is_initialised) {
+        is_initialised = true;
 
-    chThdSleepMilliseconds(10);
+        // Try releasing special pins for a short time
+        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_INPUT);
+        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_INPUT);
+        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_INPUT);
+
+        chThdSleepMilliseconds(10);
 #if defined(USE_GPIOV1)
-    palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-    palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-    palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
 #else
-    palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_ALTERNATE(SPI_SCK_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-    palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_ALTERNATE(SPI_MOSI_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-    palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_ALTERNATE(SPI_MISO_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_ALTERNATE(SPI_SCK_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_ALTERNATE(SPI_MOSI_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_ALTERNATE(SPI_MISO_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
 #endif
+    }
 }
 
 bool spi_start(pin_t slavePin, bool lsbFirst, uint8_t mode, uint16_t divisor) {
diff --git a/drivers/eeprom/eeprom_i2c.c b/drivers/eeprom/eeprom_i2c.c
index ca8af3da5..4210f06f9 100644
--- a/drivers/eeprom/eeprom_i2c.c
+++ b/drivers/eeprom/eeprom_i2c.c
@@ -42,14 +42,6 @@
 #    include "debug.h"
 #endif  // DEBUG_EEPROM_OUTPUT
 
-static inline void init_i2c_if_required(void) {
-    static int done = 0;
-    if (!done) {
-        i2c_init();
-        done = 1;
-    }
-}
-
 static inline void fill_target_address(uint8_t *buffer, const void *addr) {
     uintptr_t p = (uintptr_t)addr;
     for (int i = 0; i < EXTERNAL_EEPROM_ADDRESS_SIZE; ++i) {
@@ -58,7 +50,7 @@ static inline void fill_target_address(uint8_t *buffer, const void *addr) {
     }
 }
 
-void eeprom_driver_init(void) {}
+void eeprom_driver_init(void) { i2c_init(); }
 
 void eeprom_driver_erase(void) {
 #if defined(CONSOLE_ENABLE) && defined(DEBUG_EEPROM_OUTPUT)
@@ -80,7 +72,6 @@ void eeprom_read_block(void *buf, const void *addr, size_t len) {
     uint8_t complete_packet[EXTERNAL_EEPROM_ADDRESS_SIZE];
     fill_target_address(complete_packet, addr);
 
-    init_i2c_if_required();
     i2c_transmit(EXTERNAL_EEPROM_I2C_ADDRESS((uintptr_t)addr), complete_packet, EXTERNAL_EEPROM_ADDRESS_SIZE, 100);
     i2c_receive(EXTERNAL_EEPROM_I2C_ADDRESS((uintptr_t)addr), buf, len, 100);
 
@@ -98,7 +89,6 @@ void eeprom_write_block(const void *buf, void *addr, size_t len) {
     uint8_t * read_buf    = (uint8_t *)buf;
     uintptr_t target_addr = (uintptr_t)addr;
 
-    init_i2c_if_required();
     while (len > 0) {
         uintptr_t page_offset  = target_addr % EXTERNAL_EEPROM_PAGE_SIZE;
         int       write_length = EXTERNAL_EEPROM_PAGE_SIZE - page_offset;
diff --git a/drivers/eeprom/eeprom_spi.c b/drivers/eeprom/eeprom_spi.c
index 7b6416eaf..182731d82 100644
--- a/drivers/eeprom/eeprom_spi.c
+++ b/drivers/eeprom/eeprom_spi.c
@@ -55,14 +55,6 @@
 #    include "debug.h"
 #endif  // CONSOLE_ENABLE
 
-static void init_spi_if_required(void) {
-    static int done = 0;
-    if (!done) {
-        spi_init();
-        done = 1;
-    }
-}
-
 static bool spi_eeprom_start(void) { return spi_start(EXTERNAL_EEPROM_SPI_SLAVE_SELECT_PIN, EXTERNAL_EEPROM_SPI_LSBFIRST, EXTERNAL_EEPROM_SPI_MODE, EXTERNAL_EEPROM_SPI_CLOCK_DIVISOR); }
 
 static spi_status_t spi_eeprom_wait_while_busy(int timeout) {
@@ -91,7 +83,7 @@ static void spi_eeprom_transmit_address(uintptr_t addr) {
 
 //----------------------------------------------------------------------------------------------------------------------
 
-void eeprom_driver_init(void) {}
+void eeprom_driver_init(void) { spi_init(); }
 
 void eeprom_driver_erase(void) {
 #if defined(CONSOLE_ENABLE) && defined(DEBUG_EEPROM_OUTPUT)
@@ -110,8 +102,6 @@ void eeprom_driver_erase(void) {
 }
 
 void eeprom_read_block(void *buf, const void *addr, size_t len) {
-    init_spi_if_required();
-
     //-------------------------------------------------
     // Wait for the write-in-progress bit to be cleared
     bool res = spi_eeprom_start();
@@ -154,8 +144,6 @@ void eeprom_read_block(void *buf, const void *addr, size_t len) {
 }
 
 void eeprom_write_block(const void *buf, void *addr, size_t len) {
-    init_spi_if_required();
-
     bool      res;
     uint8_t * read_buf    = (uint8_t *)buf;
     uintptr_t target_addr = (uintptr_t)addr;
diff --git a/drivers/haptic/haptic.c b/drivers/haptic/haptic.c
index 2ce279b75..de3f40052 100644
--- a/drivers/haptic/haptic.c
+++ b/drivers/haptic/haptic.c
@@ -33,11 +33,18 @@ void haptic_init(void) {
         eeconfig_init();
     }
     haptic_config.raw = eeconfig_read_haptic();
-    if (haptic_config.mode < 1) {
-        haptic_config.mode = 1;
-    }
-    if (!haptic_config.mode) {
-        dprintf("No haptic config found in eeprom, setting default configs\n");
+#ifdef SOLENOID_ENABLE
+    solenoid_set_dwell(haptic_config.dwell);
+#endif
+    if ((haptic_config.raw == 0)
+#ifdef SOLENOID_ENABLE
+        || (haptic_config.dwell == 0)
+#endif
+    ) {
+        // this will be called, if the eeprom is not corrupt,
+        // but the previous firmware didn't have haptic enabled,
+        // or the previous firmware didn't have solenoid enabled,
+        // and the current one has solenoid enabled.
         haptic_reset();
     }
 #ifdef SOLENOID_ENABLE
@@ -118,25 +125,37 @@ void haptic_mode_decrease(void) {
 }
 
 void haptic_dwell_increase(void) {
-    uint8_t dwell = haptic_config.dwell + 1;
 #ifdef SOLENOID_ENABLE
+    int16_t next_dwell = ((int16_t)haptic_config.dwell) + SOLENOID_DWELL_STEP_SIZE;
     if (haptic_config.dwell >= SOLENOID_MAX_DWELL) {
-        dwell = 1;
+        // if it's already at max, we wrap back to min
+        next_dwell = SOLENOID_MIN_DWELL;
+    } else if (next_dwell > SOLENOID_MAX_DWELL) {
+        // if we overshoot the max, then cap at max
+        next_dwell = SOLENOID_MAX_DWELL;
     }
-    solenoid_set_dwell(dwell);
+    solenoid_set_dwell(next_dwell);
+#else
+    int16_t next_dwell = ((int16_t)haptic_config.dwell) + 1;
 #endif
-    haptic_set_dwell(dwell);
+    haptic_set_dwell(next_dwell);
 }
 
 void haptic_dwell_decrease(void) {
-    uint8_t dwell = haptic_config.dwell - 1;
 #ifdef SOLENOID_ENABLE
-    if (haptic_config.dwell < SOLENOID_MIN_DWELL) {
-        dwell = SOLENOID_MAX_DWELL;
+    int16_t next_dwell = ((int16_t)haptic_config.dwell) - SOLENOID_DWELL_STEP_SIZE;
+    if (haptic_config.dwell <= SOLENOID_MIN_DWELL) {
+        // if it's already at min, we wrap to max
+        next_dwell = SOLENOID_MAX_DWELL;
+    } else if (next_dwell < SOLENOID_MIN_DWELL) {
+        // if we go below min, then we cap to min
+        next_dwell = SOLENOID_MIN_DWELL;
     }
-    solenoid_set_dwell(dwell);
+    solenoid_set_dwell(next_dwell);
+#else
+    int16_t next_dwell = ((int16_t)haptic_config.dwell) - 1;
 #endif
-    haptic_set_dwell(dwell);
+    haptic_set_dwell(next_dwell);
 }
 
 void haptic_reset(void) {
@@ -150,6 +169,12 @@ void haptic_reset(void) {
 #ifdef SOLENOID_ENABLE
     uint8_t dwell       = SOLENOID_DEFAULT_DWELL;
     haptic_config.dwell = dwell;
+    haptic_config.buzz  = SOLENOID_DEFAULT_BUZZ;
+    solenoid_set_dwell(dwell);
+#else
+    // This is to trigger haptic_reset again, if solenoid is enabled in the future.
+    haptic_config.dwell = 0;
+    haptic_config.buzz  = 0;
 #endif
     eeconfig_update_haptic(haptic_config.raw);
     xprintf("haptic_config.feedback = %u\n", haptic_config.feedback);
diff --git a/drivers/haptic/solenoid.c b/drivers/haptic/solenoid.c
index d645c379a..2975ef893 100644
--- a/drivers/haptic/solenoid.c
+++ b/drivers/haptic/solenoid.c
@@ -32,14 +32,6 @@ void solenoid_buzz_off(void) { haptic_set_buzz(0); }
 
 void solenoid_set_buzz(int buzz) { haptic_set_buzz(buzz); }
 
-void solenoid_dwell_minus(uint8_t solenoid_dwell) {
-    if (solenoid_dwell > 0) solenoid_dwell--;
-}
-
-void solenoid_dwell_plus(uint8_t solenoid_dwell) {
-    if (solenoid_dwell < SOLENOID_MAX_DWELL) solenoid_dwell++;
-}
-
 void solenoid_set_dwell(uint8_t dwell) { solenoid_dwell = dwell; }
 
 void solenoid_stop(void) {
@@ -73,7 +65,7 @@ void solenoid_check(void) {
 
     // Check whether to buzz the solenoid on and off
     if (haptic_config.buzz) {
-        if (elapsed / SOLENOID_MIN_DWELL % 2 == 0) {
+        if ((elapsed % (SOLENOID_BUZZ_ACTUATED + SOLENOID_BUZZ_NONACTUATED)) < SOLENOID_BUZZ_ACTUATED) {
             if (!solenoid_buzzing) {
                 solenoid_buzzing = true;
                 writePinHigh(SOLENOID_PIN);
diff --git a/drivers/haptic/solenoid.h b/drivers/haptic/solenoid.h
index dd6ececa6..f2a3bc4c3 100644
--- a/drivers/haptic/solenoid.h
+++ b/drivers/haptic/solenoid.h
@@ -29,6 +29,22 @@
 #    define SOLENOID_MIN_DWELL 4
 #endif
 
+#ifndef SOLENOID_DWELL_STEP_SIZE
+#    define SOLENOID_DWELL_STEP_SIZE 1
+#endif
+
+#ifndef SOLENOID_DEFAULT_BUZZ
+#    define SOLENOID_DEFAULT_BUZZ 0
+#endif
+
+#ifndef SOLENOID_BUZZ_ACTUATED
+#    define SOLENOID_BUZZ_ACTUATED SOLENOID_MIN_DWELL
+#endif
+
+#ifndef SOLENOID_BUZZ_NONACTUATED
+#    define SOLENOID_BUZZ_NONACTUATED SOLENOID_MIN_DWELL
+#endif
+
 #ifndef SOLENOID_PIN
 #    error SOLENOID_PIN not defined
 #endif
@@ -37,8 +53,6 @@ void solenoid_buzz_on(void);
 void solenoid_buzz_off(void);
 void solenoid_set_buzz(int buzz);
 
-void solenoid_dwell_minus(uint8_t solenoid_dwell);
-void solenoid_dwell_plus(uint8_t solenoid_dwell);
 void solenoid_set_dwell(uint8_t dwell);
 
 void solenoid_stop(void);
diff --git a/drivers/oled/oled_driver.c b/drivers/oled/oled_driver.c
index ba11db1d2..53bb8ca3f 100644
--- a/drivers/oled/oled_driver.c
+++ b/drivers/oled/oled_driver.c
@@ -119,6 +119,9 @@ uint32_t oled_timeout;
 #if OLED_SCROLL_TIMEOUT > 0
 uint32_t oled_scroll_timeout;
 #endif
+#if OLED_UPDATE_INTERVAL > 0
+uint16_t oled_update_timeout;
+#endif
 
 // Internal variables to reduce math instructions
 
@@ -468,8 +471,9 @@ void oled_write_raw_byte(const char data, uint16_t index) {
 }
 
 void oled_write_raw(const char *data, uint16_t size) {
-    if (size > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE;
-    for (uint16_t i = 0; i < size; i++) {
+    uint16_t cursor_start_index = oled_cursor - &oled_buffer[0];
+    if ((size + cursor_start_index) > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE - cursor_start_index;
+    for (uint16_t i = cursor_start_index; i < cursor_start_index + size; i++) {
         if (oled_buffer[i] == data[i]) continue;
         oled_buffer[i] = data[i];
         oled_dirty |= ((OLED_BLOCK_TYPE)1 << (i / OLED_BLOCK_SIZE));
@@ -511,8 +515,9 @@ void oled_write_ln_P(const char *data, bool invert) {
 }
 
 void oled_write_raw_P(const char *data, uint16_t size) {
-    if (size > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE;
-    for (uint16_t i = 0; i < size; i++) {
+    uint16_t cursor_start_index = oled_cursor - &oled_buffer[0];
+    if ((size + cursor_start_index) > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE - cursor_start_index;
+    for (uint16_t i = cursor_start_index; i < cursor_start_index + size; i++) {
         uint8_t c = pgm_read_byte(data++);
         if (oled_buffer[i] == c) continue;
         oled_buffer[i] = c;
@@ -650,9 +655,16 @@ void oled_task(void) {
         return;
     }
 
+#if OLED_UPDATE_INTERVAL > 0
+    if (timer_elapsed(oled_update_timeout) >= OLED_UPDATE_INTERVAL) {
+        oled_update_timeout = timer_read();
+        oled_set_cursor(0, 0);
+        oled_task_user();
+    }
+#else
     oled_set_cursor(0, 0);
-
     oled_task_user();
+#endif
 
 #if OLED_SCROLL_TIMEOUT > 0
     if (oled_dirty && oled_scrolling) {