diff --git a/ChibiOS b/ChibiOS
index 669d4bb..ade76de 160000
--- a/ChibiOS
+++ b/ChibiOS
@@ -1 +1 @@
-Subproject commit 669d4bbc8da1ee0e4ccdf93a472b06d183922320
+Subproject commit ade76dea89cd093650552328e881252a06486094
diff --git a/ili9341.c b/ili9341.c
index 56c0d25..48d9984 100644
--- a/ili9341.c
+++ b/ili9341.c
@@ -323,6 +323,7 @@ void spi_init(void)
 static uint16_t current_spi_mode;
 void set_SPI_mode(uint16_t mode){
   if (current_spi_mode == mode) return;
+  dmaChannelWaitCompletionRxTx();
   // Disable current mode
   switch(current_spi_mode){
     case SPI_MODE_LCD:
diff --git a/main.c b/main.c
index 488eb7d..25798a3 100644
--- a/main.c
+++ b/main.c
@@ -2182,8 +2182,11 @@ void shell_reset_console(void){
       sduConfigureHookI(&SDU1);
   }
   // Reset I/O queue over Serial
-  oqResetI(&SD1.oqueue);
-  iqResetI(&SD1.iqueue);
+//  oqResetI(&SD1.oqueue);
+//  iqResetI(&SD1.iqueue);
+  qResetI(&SD1.oqueue);
+  qResetI(&SD1.iqueue);
+
 }