diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index b77f4f94c9..4768ec6e2f 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -691,7 +691,10 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
 */
 bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
 {
-    while (count > PKT_MAX_REGS) {
+    // The use of offset is very, very evil - it can either be a command within the page
+    // or a genuine offset, offsets within PAGE_SETUP are assumed to be commands, otherwise to be an
+    // actual offset
+    while (page != PAGE_SETUP && count > PKT_MAX_REGS) {
         if (!write_registers(page, offset, PKT_MAX_REGS, regs)) {
             return false;
         }
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 55e67dd997..2b1e65d145 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -51,6 +51,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 */
 #define IOMCU_ENABLE_RESET_TEST 0
 
+//#define IOMCU_LOOP_TIMING_DEBUG
 // enable timing GPIO pings
 #ifdef IOMCU_LOOP_TIMING_DEBUG
 #undef TOGGLE_PIN_DEBUG
@@ -103,9 +104,6 @@ static void dma_rx_end_cb(hal_uart_driver *uart)
     chSysLockFromISR();
     uart->usart->CR3 &= ~USART_CR3_DMAR;
 
-    (void)uart->usart->SR;  // sequence to clear IDLE status
-    (void)uart->usart->DR;
-    (void)uart->usart->DR;
     dmaStreamDisable(uart->dmarx);
 
     iomcu.process_io_packet();
@@ -139,8 +137,11 @@ static void dma_tx_end_cb(hal_uart_driver *uart)
     TOGGLE_PIN_DEBUG(108);
     TOGGLE_PIN_DEBUG(108);
 #endif
-
+#if AP_HAL_SHARED_DMA_ENABLED
+    chSysLockFromISR();
     chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_END);
+    chSysUnlockFromISR();
+#endif
 }
 
 /* replacement for ChibiOS uart_lld_serve_interrupt() */
@@ -154,41 +155,44 @@ static void idle_rx_handler(hal_uart_driver *uart)
               USART_SR_NE |		/* noise error - we have lost a byte due to noise */
               USART_SR_FE |
               USART_SR_PE)) {		/* framing error - start/stop bit lost or line break */
+
+        (void)uart->usart->DR;  /* SR reset step 2 - clear ORE | FE.*/
+
         /* send a line break - this will abort transmission/reception on the other end */
         chSysLockFromISR();
-
         uart->usart->SR = ~USART_SR_LBD;
         uart->usart->CR1 = cr1 | USART_CR1_SBK;
+
         iomcu.reg_status.num_errors++;
         iomcu.reg_status.err_uart++;
 
+        /* disable RX DMA */
         uart->usart->CR3 &= ~USART_CR3_DMAR;
 
-        (void)uart->usart->SR;  // clears ORE | FE
-        (void)uart->usart->DR;
-        (void)uart->usart->DR;
         setup_rx_dma(uart);
 
         chSysUnlockFromISR();
-        return;
     }
 
     if ((sr & USART_SR_TC) && (cr1 & USART_CR1_TCIE)) {
-        chSysLockFromISR();
-
         /* TC interrupt cleared and disabled.*/
         uart->usart->SR &= ~USART_SR_TC;
         uart->usart->CR1 = cr1 & ~USART_CR1_TCIE;
-
+#ifdef HAL_GPIO_LINE_GPIO105
+        TOGGLE_PIN_DEBUG(105);
+        TOGGLE_PIN_DEBUG(105);
+#endif
         /* End of transmission, a callback is generated.*/
-        _uart_tx2_isr_code(uart);
-
-        chSysUnlockFromISR();
+        dma_tx_end_cb(uart);
     }
 
-    if (sr & USART_SR_IDLE) {
+    if ((sr & USART_SR_IDLE) && (cr1 & USART_CR1_IDLEIE)) {
+        (void)uart->usart->DR;  /* SR reset step 2 - clear IDLE.*/
+
         /* the DMA size is the maximum packet size, but smaller packets are perfectly possible leading to 
            an IDLE ISR. The data still must be processed. */
+
+        /* End of receive, a callback is generated.*/
         dma_rx_end_cb(uart);
     }
 }
@@ -248,9 +252,9 @@ static UARTConfig uart_cfg = {
     dma_tx_end_cb,
     dma_rx_end_cb,
     nullptr,
-    nullptr,
-    idle_rx_handler,
-    nullptr,
+    nullptr,            // error
+    idle_rx_handler,    // global irq
+    nullptr,            // idle
     1500000,      //1.5MBit
     USART_CR1_IDLEIE,
     0,
@@ -660,6 +664,12 @@ void AP_IOMCU_FW::process_io_packet()
 {
     iomcu.reg_status.total_pkts++;
 
+    if (rx_io_packet.code == CODE_NOOP) {
+        iomcu.reg_status.num_errors++;
+        iomcu.reg_status.err_bad_opcode++;
+        return;
+    }
+
     uint8_t rx_crc = rx_io_packet.crc;
     uint8_t calc_crc;
     rx_io_packet.crc = 0;
@@ -714,9 +724,18 @@ void AP_IOMCU_FW::process_io_packet()
     default: {
         iomcu.reg_status.num_errors++;
         iomcu.reg_status.err_bad_opcode++;
+        rx_io_packet.code = CODE_NOOP;
+        rx_io_packet.count = 0;
+        return;
     }
     break;
     }
+
+    // prevent a spurious DMA callback from doing anything bad
+    rx_io_packet.code = CODE_NOOP;
+    rx_io_packet.count = 0;
+
+    return;
 }
 
 /*
@@ -942,7 +961,7 @@ bool AP_IOMCU_FW::handle_code_write()
             // no input when override is active
             break;
         }
-        if (rx_io_packet.count != sizeof(reg_direct_pwm.pwm)/2) {
+        if (rx_io_packet.count > sizeof(reg_direct_pwm.pwm)/2) {
             return false;
         }
         /* copy channel data */
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index 01a29b15ff..9e13fafc48 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -37,6 +37,7 @@ enum iocode {
     // read types
     CODE_READ = 0,
     CODE_WRITE = 1,
+    CODE_NOOP = 2,
 
     // reply codes
     CODE_SUCCESS = 0,