diff --git a/include/adc.h b/include/adc.h
index e9d25ae..1eba5a5 100644
--- a/include/adc.h
+++ b/include/adc.h
@@ -21,6 +21,6 @@ struct adc_state {
 extern struct adc_state st_adc;
 
 void adc_init(void);
-void adc_dma_interrupt(uint32_t channel, uint32_t flags);
+void adc_dma_interrupt(DMA_Channel_TypeDef *channel, uint32_t flags);
 
 #endif /* __ADC_H__ */
diff --git a/include/global.h b/include/global.h
index 7c556bb..a781ff2 100644
--- a/include/global.h
+++ b/include/global.h
@@ -14,7 +14,6 @@
 #include <stm32f0xx.h>
 #include <core_cm0.h>
 
-#define DMA1_Channel       DMA1_Channel1
 #define DMA_ISR_FLAGS_Pos(channel) (4 * ((channel) - 1))
 #define DMA_ISR_FLAGS_CH(channel) (0xf << DMA_ISR_FLAGS_Pos(channel))
 
@@ -57,6 +56,8 @@ enum ErrorCode {
 typedef enum ErrorCode ErrorCode;
 
 enum board_config {
+    BCFG_UNCONFIGURED = 0,
+
     /* The board assumes one of three configurations depending on connected periphery.
      *
      */
@@ -101,7 +102,7 @@ void delay_us(int duration_us);
 uint64_t get_sync_time(void);
 
 extern enum board_config board_config;
-extern uint64_t sys_time_us;
-extern uint64_t sync_time_us;
+extern volatile uint64_t sys_time_us;
+extern volatile uint64_t sync_time_us;
 
 #endif /* __GLOBAL_H__ */
diff --git a/include/lcd.h b/include/lcd.h
index b489219..6d7aa8f 100644
--- a/include/lcd.h
+++ b/include/lcd.h
@@ -1,7 +1,10 @@
 #ifndef __LCD_H__
 #define __LCD_H__
 
+#include <global.h>
+
 void lcd_init(void);
+void lcd_uninit(void);
 void lcd_write_dma(const char **buf);
 
 #ifndef LCD_PEX_ADDR
@@ -18,6 +21,6 @@ struct lcd_state {
 
 extern struct lcd_state st_lcd;
 
-void lcd_dma_interrupt(uint32_t channel, uint32_t flags);
+void lcd_dma_interrupt(DMA_Channel_TypeDef *channel, uint32_t flags);
 
 #endif /* __LCD_H__ */
diff --git a/include/proto.h b/include/proto.h
index 754db9c..81ea7a5 100644
--- a/include/proto.h
+++ b/include/proto.h
@@ -30,7 +30,6 @@ enum packet_type {
 struct proto_state {
     enum proto_fsm fsm;
     uint64_t last_start;
-    int delta;
     ErrorCode error;
     uint64_t last_error;
     bool timeout[3];
diff --git a/include/serial.h b/include/serial.h
index 1d05155..072436d 100644
--- a/include/serial.h
+++ b/include/serial.h
@@ -33,7 +33,7 @@ struct uart_dma_state {
 
     struct cobs_decode_state cobs_state;
 
-    size_t tx_dma_ch;
+    DMA_Channel_TypeDef *tx_dma_ch;
     USART_TypeDef *usart;
 
     uint8_t rx_buf[32];
@@ -54,10 +54,10 @@ struct __attribute__((__packed__)) ll_pkt {
 
 extern volatile struct uart_dma_state uart_st;
 
-void uart_dma_init(USART_TypeDef *usart, int tx_dma_ch, int addr);
+void uart_dma_init(USART_TypeDef *usart, DMA_Channel_TypeDef *tx_dma_ch, int addr);
 int uart_send_packet_nonblocking(struct ll_pkt *pkt, size_t pkt_len);
 int uart_ack_packet(uint8_t idx);
-void uart_dma_interrupt(uint32_t channel, uint32_t flags);
+void uart_dma_interrupt(DMA_Channel_TypeDef *ch, uint32_t flags);
 void uart_interrupt(uint32_t isr);
 
 extern void uart_handle_user_packet(struct ll_pkt *pkt, size_t len);
diff --git a/src/adc.c b/src/adc.c
index aa760bb..09dd8b2 100644
--- a/src/adc.c
+++ b/src/adc.c
@@ -135,21 +135,18 @@ void adc_init() {
     adc_sync_null();
     adc_cs(1);
 
-    NVIC_EnableIRQ(DMA1_Channel1_IRQn);
-    NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);
-
     GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER2_Msk) | (2<<GPIO_MODER_MODER2_Pos); /* CS */
     DMA1_Channel1->CCR |= DMA_CCR_EN;
     DMA1_Channel2->CCR |= DMA_CCR_EN;
     TIM2->SMCR = TIM_SMCR_ETP | (7<<TIM_SMCR_TS_Pos) | (6<<TIM_SMCR_SMS_Pos);
 }
 
-void adc_dma_interrupt(uint32_t channel, uint32_t flags) {
+void adc_dma_interrupt(DMA_Channel_TypeDef *channel, uint32_t flags) {
     if (flags & DMA_ISR_TEIF1) {
         st_adc.error = ERR_DMA;
         st_adc.last_error = sys_time_us;
 
-    } else if (flags & DMA_ISR_TCIF1 && channel == 3) {
+    } else if (flags & DMA_ISR_TCIF1 && channel == DMA1_Channel3) {
         DMA1_Channel3->CCR &= ~DMA_CCR_EN;
 
         if ((st_adc.rxbuf[0] & 0x3) != 0x1) {
diff --git a/src/lcd.c b/src/lcd.c
index 72231b6..4362193 100644
--- a/src/lcd.c
+++ b/src/lcd.c
@@ -13,6 +13,13 @@ static ErrorCode lcd_pex_set(uint8_t value);
 static ErrorCode lcd_command(uint8_t value);
 
 
+void lcd_uninit() {
+    SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_I2C1_DMA_RMP | SYSCFG_CFGR1_TIM17_DMA_RMP2);
+    TIM17->CR1 &= ~TIM_CR1_CEN;
+    NVIC_DisableIRQ(I2C1_IRQn);
+}
+
+
 void i2c_init() {
     SYSCFG->CFGR1 |= SYSCFG_CFGR1_I2C1_DMA_RMP | SYSCFG_CFGR1_TIM17_DMA_RMP2;
 
@@ -39,7 +46,6 @@ void i2c_init() {
     DMA1_Channel7->CPAR = (uint32_t)&(I2C1->CR2);
     DMA1_Channel7->CMAR = (uint32_t)&st_lcd.cr2_buf;
 
-    NVIC_EnableIRQ(DMA1_Channel4_5_6_7_IRQn);
     NVIC_EnableIRQ(I2C1_IRQn);
 }
 
@@ -143,6 +149,7 @@ void lcd_init() {
     int rc = lcd_tx_nibble(LCD_ENTER_8BIT, 0);
     if (rc) {
         st_lcd.error = rc;
+        lcd_uninit();
         return;
     }
     delay_us(4100);
@@ -195,7 +202,7 @@ void lcd_init() {
     st_lcd.led = true;
 }
 
-void lcd_dma_interrupt(uint32_t channel, uint32_t flags) {
+void lcd_dma_interrupt(DMA_Channel_TypeDef *channel, uint32_t flags) {
     if (flags & DMA_ISR_TEIF1) {
         st_lcd.error = ERR_DMA;
     }
diff --git a/src/main.c b/src/main.c
index 9548fd1..bcb2af9 100644
--- a/src/main.c
+++ b/src/main.c
@@ -18,8 +18,8 @@ static void loop_adc(void);
 
 
 enum board_config board_config;
-uint64_t sys_time_us;
-uint64_t sync_time_us;
+volatile uint64_t sys_time_us;
+volatile uint64_t sync_time_us;
 
 
 int main(void) {
@@ -165,7 +165,7 @@ int main(void) {
     lcd_init();
 
     if (st_lcd.has_lcd) {
-        uart_dma_init(USART3, 2, 2);
+        uart_dma_init(USART3, DMA1_Channel7, BADDR_DISPLAY);
         const char *lines[4] = {"Display initialized ",
                                 "                    ",
                                 "Waiting for         ",
@@ -177,26 +177,24 @@ int main(void) {
         set_led(LED_DISPLAY, 0, 255, 0); /* green */
         set_led(LED_BUTTONS, 0, 255, 0); /* green */
         board_config = BCFG_DISPLAY;
-        uart_st.addr = BADDR_DISPLAY;
 
         blank_7seg(0);
         blank_7seg(1);
         blank_7seg(2);
 
     } else {
-        uart_dma_init(USART3, 7, 1);
         int bt = bt_inputs();
         if (bt&1) {
+            uart_dma_init(USART3, DMA1_Channel7, BADDR_MOTOR);
             set_led(LED_DISPLAY, 0, 0, 255); /* blue */
             set_led(LED_BUTTONS, 0, 0, 255); /* blue */
             board_config = BCFG_MOTOR;
-            uart_st.addr = BADDR_MOTOR;
 
         } else {
+            uart_dma_init(USART3, DMA1_Channel7, BADDR_MEAS_BASE + ((bt>>1) & 3));
             set_led(LED_DISPLAY, 255, 0, 0); /* red */
             set_led(LED_BUTTONS, 0, 255, 255); /* cyan */
             board_config = BCFG_MEAS;
-            uart_st.addr = BADDR_MEAS_BASE + ((bt>>1) & 3);
         }
     }
 
@@ -208,6 +206,7 @@ int main(void) {
     SysTick->LOAD = SystemCoreClock / SYSTICK_INTERVAL_US; /* 1ms interrupt interval */
     SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;
     NVIC_EnableIRQ(SysTick_IRQn);
+    NVIC_SetPriority(SysTick_IRQn, 0<<5);
 
     /* Either for UART or ADC */
     NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);
@@ -217,6 +216,10 @@ int main(void) {
     NVIC_EnableIRQ(USART3_4_IRQn);
     NVIC_SetPriority(USART3_4_IRQn, 1<<5);
 
+    /* LCD and UART TX */
+    NVIC_EnableIRQ(DMA1_Channel4_5_6_7_IRQn);
+    NVIC_SetPriority(DMA1_Channel4_5_6_7_IRQn, 1<<5);
+
     int n = 0;
     while (23) {
         n++;
@@ -380,16 +383,16 @@ void DMA1_Channel2_3_IRQHandler() {
 
     if (flags & DMA_ISR_FLAGS_CH(2)) {
         if (board_config == BCFG_MEAS) {
-            adc_dma_interrupt(2, flags>>DMA_ISR_FLAGS_Pos(2));
-        } else {
-            uart_dma_interrupt(2, flags>>DMA_ISR_FLAGS_Pos(2));
+            adc_dma_interrupt(DMA1_Channel2, flags>>DMA_ISR_FLAGS_Pos(2));
+        } else if (board_config == BCFG_DISPLAY) {
+            uart_dma_interrupt(DMA1_Channel2, flags>>DMA_ISR_FLAGS_Pos(2));
         }
 
     }
 
     if (flags & DMA_ISR_FLAGS_CH(3)) {
         if (board_config == BCFG_MEAS) {
-            adc_dma_interrupt(3, flags>>DMA_ISR_FLAGS_Pos(3));
+            adc_dma_interrupt(DMA1_Channel3, flags>>DMA_ISR_FLAGS_Pos(3));
         }
     }
 }
@@ -400,15 +403,15 @@ void DMA1_Channel4_5_6_7_IRQHandler() {
 
     if (flags & DMA_ISR_FLAGS_CH(6)) {
         if (board_config == BCFG_DISPLAY) {
-            lcd_dma_interrupt(6, flags>>DMA_ISR_FLAGS_Pos(6));
+            lcd_dma_interrupt(DMA1_Channel6, flags>>DMA_ISR_FLAGS_Pos(6));
         }
     }
 
     if (flags & DMA_ISR_FLAGS_CH(7)) {
         if (board_config == BCFG_DISPLAY) {
-            lcd_dma_interrupt(6, flags>>DMA_ISR_FLAGS_Pos(7));
+            lcd_dma_interrupt(DMA1_Channel7, flags>>DMA_ISR_FLAGS_Pos(7));
         } else {
-            uart_dma_interrupt(7, flags>>DMA_ISR_FLAGS_Pos(7));
+            uart_dma_interrupt(DMA1_Channel7, flags>>DMA_ISR_FLAGS_Pos(7));
         }
     }
 }
diff --git a/src/proto.c b/src/proto.c
index 07eee5d..32c409d 100644
--- a/src/proto.c
+++ b/src/proto.c
@@ -19,7 +19,7 @@ void proto_init() {
 
 void proto_loop() {
     if (board_config == BCFG_MOTOR) { /* We are the protocol host */
-        if (sys_time_us - st_proto.last_start > PROTO_UPDATE_INTERVAL_MS*1000UL - st_proto.delta ) {
+        if (sys_time_us - st_proto.last_start > PROTO_UPDATE_INTERVAL_MS*1000UL) {
             st_proto.fsm = ST_START;
             proto_handle_host(false);
 
@@ -68,7 +68,7 @@ void proto_handle_host(bool timeout) {
     ErrorCode rc = ERR_SUCCESS;
     switch (st_proto.fsm) {
         case ST_START:
-            st_proto.delta += (sys_time_us - st_proto.last_start) - PROTO_UPDATE_INTERVAL_MS*1000;
+            //st_proto.delta += (sys_time_us - st_proto.last_start) - PROTO_UPDATE_INTERVAL_MS*1000;
             st_proto.last_start = sys_time_us;
             tx_time_sync();
             st_proto.fsm = ST_TIME_SYNC;
diff --git a/src/serial.c b/src/serial.c
index 431c223..f932efa 100644
--- a/src/serial.c
+++ b/src/serial.c
@@ -23,15 +23,15 @@ void uart_dma_reset() {
     cobs_decode_incremental_initialize((struct cobs_decode_state *)&uart_st.cobs_state);
 }
 
-void uart_dma_init(USART_TypeDef *usart, int tx_dma_ch, int addr) {
+void uart_dma_init(USART_TypeDef *usart, DMA_Channel_TypeDef *tx_dma_ch, int addr) {
     uart_dma_reset();
     uart_st.tx_dma_ch = tx_dma_ch;
     uart_st.usart = usart;
     uart_st.addr = addr;
 
     /* Configure DMA 1 Channel 2 to handle uart transmission */
-    DMA1_Channel[uart_st.tx_dma_ch].CPAR = (uint32_t)&(USART1->TDR);
-    DMA1_Channel[uart_st.tx_dma_ch].CCR = (0<<DMA_CCR_PL_Pos)
+    uart_st.tx_dma_ch->CPAR = (uint32_t)&(usart->TDR);
+    uart_st.tx_dma_ch->CCR = (0<<DMA_CCR_PL_Pos)
         | DMA_CCR_DIR
         | (0<<DMA_CCR_MSIZE_Pos) /* 8 bit */
         | (0<<DMA_CCR_PSIZE_Pos) /* 8 bit */
@@ -41,16 +41,17 @@ void uart_dma_init(USART_TypeDef *usart, int tx_dma_ch, int addr) {
     usart->CR1 = /* 8-bit -> M1, M0 clear */
         /* OVER8 clear. Use default 16x oversampling */
         /* CMIF clear */
-          USART_CR1_MME
+        /* MME clear */
         /* WAKE clear */
         /* PCE, PS clear */
-        | USART_CR1_RXNEIE /* Enable receive interrupt */
+          USART_CR1_RXNEIE /* Enable receive interrupt */
         /* other interrupts clear */
         | USART_CR1_TE
         | USART_CR1_RE;
     /* Set baudrate */
     usart->BRR = 192; /* 250kBd */
-    usart->CR3 |= USART_CR3_DMAT; /* TX DMA enable */
+    usart->CR3 |= USART_CR3_DMAT /* TX DMA enable */
+        | USART_CR3_DEM; /* Output DE signal on RTS pin */
 
     /* And... go! */
     usart->CR1 |= USART_CR1_UE;
@@ -92,9 +93,6 @@ void uart_interrupt(uint32_t isr) {
 
         uart_st.last_packet = sys_time_us;
         uart_handle_user_packet((struct ll_pkt *)pkt, rc);
-        if (!(DMA1_Channel[uart_st.tx_dma_ch].CCR & DMA_CCR_EN)) {
-            uart_schedule_dma(false);
-        }
     }
 }
 
@@ -150,10 +148,15 @@ void uart_schedule_dma(bool retransmit) {
     buf->xfr_start = xfr_start;
     buf->xfr_end = xfr_end;
 
+    /* Wait for channel to turn off */
+    while (uart_st.tx_dma_ch->CCR & DMA_CCR_EN) {
+        /* do nothing */
+    }
+
     /* initiate transmission of new buffer */
-    DMA1_Channel[uart_st.tx_dma_ch].CMAR = (uint32_t)(buf->data + xfr_start);
-    DMA1_Channel[uart_st.tx_dma_ch].CNDTR = xfr_len;
-    DMA1_Channel[uart_st.tx_dma_ch].CCR |= DMA_CCR_EN;
+    uart_st.tx_dma_ch->CMAR = (uint32_t)(buf->data + xfr_start);
+    uart_st.tx_dma_ch->CNDTR = xfr_len;
+    uart_st.tx_dma_ch->CCR |= DMA_CCR_EN;
 }
 
 int uart_putc_nonblocking(uint8_t c) {
@@ -168,22 +171,15 @@ int uart_putc_nonblocking(uint8_t c) {
 }
 
 
-void uart_dma_interrupt(uint32_t channel, uint32_t flags) {
-    DMA1_Channel[uart_st.tx_dma_ch].CCR &= ~DMA_CCR_EN;
+void uart_dma_interrupt(DMA_Channel_TypeDef *channel, uint32_t flags) {
+    uart_st.tx_dma_ch->CCR &= ~DMA_CCR_EN;
 
     if (flags & DMA_ISR_TEIF1) {
         uart_st.error = ERR_DMA;
         uart_st.last_error = sys_time_us;
 
     } else if (flags & DMA_ISR_TCIF1 && channel == uart_st.tx_dma_ch) {
-        /* Wait for channel to turn off */
-        while (DMA1_Channel[uart_st.tx_dma_ch].CCR & DMA_CCR_EN) {
-            /* do nothing */
-        }
-
-        if (uart_st.tx_buf.wraparound) {
-            uart_schedule_dma(false);
-        }
+        uart_schedule_dma(false);
     }
 }
 
@@ -210,7 +206,7 @@ int uart_send_packet_nonblocking(struct ll_pkt *pkt, size_t pkt_len) {
 
     uart_st.packet_count++;
 
-    if (!(DMA1_Channel[uart_st.tx_dma_ch].CCR & DMA_CCR_EN))
+    if (!(uart_st.tx_dma_ch->CCR & DMA_CCR_EN))
         uart_schedule_dma(false);
     return 0;
 }