diff --git a/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.c b/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.c
index 3c8c29db98..99ab0762d8 100644
--- a/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.c
+++ b/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.c
@@ -533,11 +533,11 @@ static bool_t otg_txfifo_handler(USBDriver *usbp, usbep_t ep) {
                                  n);
       usbp->epc[ep]->in_state->mode.linear.txbuf += n;
     }
-    usbp->epc[ep]->in_state->txcnt += n;
-  }
 #if STM32_USB_OTGFIFO_FILL_BASEPRI
   __set_BASEPRI(0);
 #endif
+    usbp->epc[ep]->in_state->txcnt += n;
+  }
 }
 
 /**
diff --git a/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.h b/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.h
index e8ff68fb0f..802f13a222 100644
--- a/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.h
+++ b/firmware/chibios/os/hal/platforms/STM32/OTGv1/usb_lld.h
@@ -52,6 +52,11 @@
  */
 #define USB_SET_ADDRESS_MODE                USB_EARLY_SET_ADDRESS
 
+/**
+ * @brief   Method for set address acknowledge.
+ */
+#define USB_SET_ADDRESS_ACK_HANDLING        USB_SET_ADDRESS_ACK_SW
+
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
 /*===========================================================================*/
diff --git a/firmware/chibios/os/hal/platforms/STM32/USARTv1/uart_lld.c b/firmware/chibios/os/hal/platforms/STM32/USARTv1/uart_lld.c
index 81dfa8bc3e..2e40309359 100644
--- a/firmware/chibios/os/hal/platforms/STM32/USARTv1/uart_lld.c
+++ b/firmware/chibios/os/hal/platforms/STM32/USARTv1/uart_lld.c
@@ -223,11 +223,10 @@ static void usart_start(UARTDriver *uartp) {
   u->CR2 = uartp->config->cr2 | USART_CR2_LBDIE;
   u->CR3 = uartp->config->cr3 | USART_CR3_DMAT | USART_CR3_DMAR |
                                 USART_CR3_EIE;
-  if (uartp->config->txend2_cb == NULL)
-    cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE;
-  else
-    cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE |
-          USART_CR1_TCIE;
+
+  /* Mustn't ever set TCIE here - if done, it causes an immediate
+     interrupt.*/
+  cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE;
   u->CR1 = uartp->config->cr1 | cr1;
 
   /* Starting the receiver idle loop.*/
@@ -293,6 +292,12 @@ static void uart_lld_serve_tx_end_irq(UARTDriver *uartp, uint32_t flags) {
 
   dmaStreamDisable(uartp->dmatx);
 
+  /* Only enable TC interrupt if there's a callback attached to it.
+     We have to do it here, rather than earlier, because TC flag is set
+     until transmission starts.*/
+  if (uartp->config->txend2_cb != NULL)
+    uartp->usart->CR1 |= USART_CR1_TCIE;
+
   /* A callback is generated, if enabled, after a completed transfer.*/
   uartp->txstate = UART_TX_COMPLETE;
   if (uartp->config->txend1_cb != NULL)
@@ -312,17 +317,22 @@ static void uart_lld_serve_tx_end_irq(UARTDriver *uartp, uint32_t flags) {
 static void serve_usart_irq(UARTDriver *uartp) {
   uint16_t sr;
   USART_TypeDef *u = uartp->usart;
-  
+  uint32_t cr1 = u->CR1;
+
   sr = u->SR;   /* SR reset step 1.*/
   (void)u->DR;  /* SR reset step 2.*/
+
   if (sr & (USART_SR_LBD | USART_SR_ORE | USART_SR_NE |
             USART_SR_FE  | USART_SR_PE)) {
     u->SR = ~USART_SR_LBD;
     if (uartp->config->rxerr_cb != NULL)
       uartp->config->rxerr_cb(uartp, translate_errors(sr));
   }
-  if (sr & USART_SR_TC) {
+
+  if ((sr & USART_SR_TC) && (cr1 & USART_CR1_TCIE)) {
+    /* TC interrupt cleared and disabled.*/
     u->SR = ~USART_SR_TC;
+    u->CR1 = cr1 & ~USART_CR1_TCIE;
 
     /* End of transmission, a callback is generated.*/
     if (uartp->config->txend2_cb != NULL)
diff --git a/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c b/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c
index 37305bbee0..72a0857e18 100644
--- a/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c
+++ b/firmware/chibios/os/hal/platforms/STM32/USARTv2/uart_lld.c
@@ -177,11 +177,10 @@ static void usart_start(UARTDriver *uartp) {
   u->CR2 = uartp->config->cr2 | USART_CR2_LBDIE;
   u->CR3 = uartp->config->cr3 | USART_CR3_DMAT | USART_CR3_DMAR |
                                 USART_CR3_EIE;
-  if (uartp->config->txend2_cb == NULL)
-    cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE;
-  else
-    cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE |
-          USART_CR1_TCIE;
+
+  /* Mustn't ever set TCIE here - if done, it causes an immediate
+     interrupt.*/
+  cr1 = USART_CR1_UE | USART_CR1_PEIE | USART_CR1_TE | USART_CR1_RE;
   u->CR1 = uartp->config->cr1 | cr1;
 
   /* Starting the receiver idle loop.*/
@@ -247,6 +246,12 @@ static void uart_lld_serve_tx_end_irq(UARTDriver *uartp, uint32_t flags) {
 
   dmaStreamDisable(uartp->dmatx);
 
+  /* Only enable TC interrupt if there's a callback attached to it.
+     We have to do it here, rather than earlier, because TC flag is set
+     until transmission starts.*/
+  if (uartp->config->txend2_cb != NULL)
+    uartp->usart->CR1 |= USART_CR1_TCIE;
+
   /* A callback is generated, if enabled, after a completed transfer.*/
   uartp->txstate = UART_TX_COMPLETE;
   if (uartp->config->txend1_cb != NULL)
@@ -266,6 +271,7 @@ static void uart_lld_serve_tx_end_irq(UARTDriver *uartp, uint32_t flags) {
 static void serve_usart_irq(UARTDriver *uartp) {
   uint32_t isr;
   USART_TypeDef *u = uartp->usart;
+  uint32_t cr1 = u->CR1;
   
   /* Reading and clearing status.*/
   isr = u->ISR;
@@ -276,7 +282,11 @@ static void serve_usart_irq(UARTDriver *uartp) {
     if (uartp->config->rxerr_cb != NULL)
       uartp->config->rxerr_cb(uartp, translate_errors(isr));
   }
-  if (isr & USART_ISR_TC) {
+
+  if ((isr & USART_ISR_TC) && (cr1 & USART_CR1_TCIE))  {
+    /* TC interrupt disabled.*/
+    u->CR1 = cr1 & ~USART_CR1_TCIE;
+
     /* End of transmission, a callback is generated.*/
     if (uartp->config->txend2_cb != NULL)
       uartp->config->txend2_cb(uartp);
diff --git a/firmware/chibios/os/hal/platforms/STM32/USBv1/usb_lld.h b/firmware/chibios/os/hal/platforms/STM32/USBv1/usb_lld.h
index aa1905380c..405b0c8e4f 100644
--- a/firmware/chibios/os/hal/platforms/STM32/USBv1/usb_lld.h
+++ b/firmware/chibios/os/hal/platforms/STM32/USBv1/usb_lld.h
@@ -48,6 +48,11 @@
  */
 #define USB_SET_ADDRESS_MODE                USB_LATE_SET_ADDRESS
 
+/**
+ * @brief   Method for set address acknowledge.
+ */
+#define USB_SET_ADDRESS_ACK_HANDLING        USB_SET_ADDRESS_ACK_SW
+
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
 /*===========================================================================*/
diff --git a/firmware/chibios/os/hal/platforms/STM32F1xx/stm32_rcc.h b/firmware/chibios/os/hal/platforms/STM32F1xx/stm32_rcc.h
index 786cec07b0..4cb73e3755 100644
--- a/firmware/chibios/os/hal/platforms/STM32F1xx/stm32_rcc.h
+++ b/firmware/chibios/os/hal/platforms/STM32F1xx/stm32_rcc.h
@@ -299,6 +299,31 @@
  * @api
  */
 #define rccResetCAN1() rccResetAPB1(RCC_APB1RSTR_CAN1RST)
+
+/**
+ * @brief   Enables the CAN2 peripheral clock.
+ *
+ * @param[in] lp        low power enable flag
+ *
+ * @api
+ */
+#define rccEnableCAN2(lp) rccEnableAPB1(RCC_APB1ENR_CAN2EN, lp)
+
+/**
+ * @brief   Disables the CAN2 peripheral clock.
+ *
+ * @param[in] lp        low power enable flag
+ *
+ * @api
+ */
+#define rccDisableCAN2(lp) rccDisableAPB1(RCC_APB1ENR_CAN2EN, lp)
+
+/**
+ * @brief   Resets the CAN2 peripheral.
+ *
+ * @api
+ */
+#define rccResetCAN2() rccResetAPB1(RCC_APB1RSTR_CAN2RST)
 /** @} */
 
 /**
diff --git a/firmware/chibios/os/hal/src/mmc_spi.c b/firmware/chibios/os/hal/src/mmc_spi.c
index 19a2173ccb..05454f71d6 100644
--- a/firmware/chibios/os/hal/src/mmc_spi.c
+++ b/firmware/chibios/os/hal/src/mmc_spi.c
@@ -119,7 +119,7 @@ static bool_t mmc_read(void *instance, uint32_t startblk,
     n--;
   }
   if (mmcStopSequentialRead((MMCDriver *)instance))
-      return CH_FAILED;
+    return CH_FAILED;
   return CH_SUCCESS;
 }
 
@@ -127,15 +127,15 @@ static bool_t mmc_write(void *instance, uint32_t startblk,
                  const uint8_t *buffer, uint32_t n) {
 
   if (mmcStartSequentialWrite((MMCDriver *)instance, startblk))
-      return CH_FAILED;
+    return CH_FAILED;
   while (n > 0) {
-      if (mmcSequentialWrite((MMCDriver *)instance, buffer))
-          return CH_FAILED;
-      buffer += MMCSD_BLOCK_SIZE;
-      n--;
+    if (mmcSequentialWrite((MMCDriver *)instance, buffer))
+        return CH_FAILED;
+    buffer += MMCSD_BLOCK_SIZE;
+    n--;
   }
   if (mmcStopSequentialWrite((MMCDriver *)instance))
-      return CH_FAILED;
+    return CH_FAILED;
   return CH_SUCCESS;
 }
 
@@ -175,7 +175,7 @@ static void wait(MMCDriver *mmcp) {
     spiReceive(mmcp->config->spip, 1, buf);
     if (buf[0] == 0xFF)
       break;
-#ifdef MMC_NICE_WAITING
+#if MMC_NICE_WAITING
     /* Trying to be nice with the other threads.*/
     chThdSleep(1);
 #endif
@@ -355,7 +355,7 @@ static void sync(MMCDriver *mmcp) {
     spiReceive(mmcp->config->spip, 1, buf);
     if (buf[0] == 0xFF)
       break;
-#ifdef MMC_NICE_WAITING
+#if MMC_NICE_WAITING
     chThdSleep(1);      /* Trying to be nice with the other threads.*/
 #endif
   }
diff --git a/firmware/chibios/os/kernel/include/ch.h b/firmware/chibios/os/kernel/include/ch.h
index 65391f9474..c2a14646aa 100644
--- a/firmware/chibios/os/kernel/include/ch.h
+++ b/firmware/chibios/os/kernel/include/ch.h
@@ -47,7 +47,7 @@
 /**
  * @brief   Kernel version string.
  */
-#define CH_KERNEL_VERSION       "2.6.7"
+#define CH_KERNEL_VERSION       "2.6.8"
 
 /**
  * @name    Kernel version
@@ -66,7 +66,7 @@
 /**
  * @brief   Kernel version patch number.
  */
-#define CH_KERNEL_PATCH         7
+#define CH_KERNEL_PATCH         8
 /** @} */
 
 /**
diff --git a/firmware/chibios/os/kernel/src/chmtx.c b/firmware/chibios/os/kernel/src/chmtx.c
index 1fac622e53..08b7de5885 100644
--- a/firmware/chibios/os/kernel/src/chmtx.c
+++ b/firmware/chibios/os/kernel/src/chmtx.c
@@ -142,8 +142,8 @@ void chMtxLockS(Mutex *mp) {
         prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
         tp = ((Mutex *)tp->p_u.wtobjp)->m_owner;
         continue;
-#if CH_USE_CONDVARS |                                                       \
-    (CH_USE_SEMAPHORES && CH_USE_SEMAPHORES_PRIORITY) |                     \
+#if CH_USE_CONDVARS ||                                                      \
+    (CH_USE_SEMAPHORES && CH_USE_SEMAPHORES_PRIORITY) ||                    \
     (CH_USE_MESSAGES && CH_USE_MESSAGES_PRIORITY)
 #if CH_USE_CONDVARS
       case THD_STATE_WTCOND:
diff --git a/firmware/chibios/os/kernel/src/chqueues.c b/firmware/chibios/os/kernel/src/chqueues.c
index e8a33cf753..22986badef 100644
--- a/firmware/chibios/os/kernel/src/chqueues.c
+++ b/firmware/chibios/os/kernel/src/chqueues.c
@@ -391,7 +391,7 @@ msg_t chOQGetI(OutputQueue *oqp) {
  *          buffer.
  *
  * @param[in] oqp       pointer to an @p OutputQueue structure
- * @param[out] bp       pointer to the data buffer
+ * @param[in] bp        pointer to the data buffer
  * @param[in] n         the maximum amount of data to be transferred, the
  *                      value 0 is reserved
  * @param[in] time      the number of ticks before the operation timeouts,
diff --git a/firmware/chibios/os/kernel/src/chregistry.c b/firmware/chibios/os/kernel/src/chregistry.c
index 05818d95b4..36c5133c23 100644
--- a/firmware/chibios/os/kernel/src/chregistry.c
+++ b/firmware/chibios/os/kernel/src/chregistry.c
@@ -69,7 +69,7 @@ ROMCONST chdebug_t ch_debug = {
   (uint8_t)sizeof (chdebug_t),
   (uint16_t)((CH_KERNEL_MAJOR << 11) |
              (CH_KERNEL_MINOR << 6) |
-             (CH_KERNEL_PATCH) << 0),
+             (CH_KERNEL_PATCH << 0)),
   (uint8_t)sizeof (void *),
   (uint8_t)sizeof (systime_t),
   (uint8_t)sizeof (Thread),
diff --git a/firmware/chibios/os/kernel/src/chsys.c b/firmware/chibios/os/kernel/src/chsys.c
index 41158c9299..a91dddff55 100644
--- a/firmware/chibios/os/kernel/src/chsys.c
+++ b/firmware/chibios/os/kernel/src/chsys.c
@@ -123,8 +123,6 @@ void chSysInit(void) {
 #endif
 }
 
-void assertVtList(void);
-
 /**
  * @brief   Handles time ticks for round robin preemption and timer increments.
  * @details Decrements the remaining time quantum of the running thread
@@ -149,7 +147,6 @@ void chSysTimerHandlerI(void) {
 #if CH_DBG_THREADS_PROFILING
   currp->p_time++;
 #endif
-//  assertVtList();
   chVTDoTickI();
 #if defined(SYSTEM_TICK_EVENT_HOOK)
   SYSTEM_TICK_EVENT_HOOK();
diff --git a/firmware/chibios/os/kernel/src/chvt.c b/firmware/chibios/os/kernel/src/chvt.c
index c33398e94d..c1c694560d 100644
--- a/firmware/chibios/os/kernel/src/chvt.c
+++ b/firmware/chibios/os/kernel/src/chvt.c
@@ -77,9 +77,6 @@ void chVTSetI(VirtualTimer *vtp, systime_t time, vtfunc_t vtfunc, void *par) {
   VirtualTimer *p;
 
   chDbgCheckClassI();
-  chDbgCheck(vtp != NULL, "chVTSetI 1");
-  chDbgCheck(vtfunc != NULL, "chVTSetI 2");
-  chDbgCheck(time != TIME_IMMEDIATE, "chVTSetI 3");
   chDbgCheck((vtp != NULL) && (vtfunc != NULL) && (time != TIME_IMMEDIATE),
              "chVTSetI");
 
diff --git a/firmware/config/stm32f4ems/chconf.h b/firmware/config/stm32f4ems/chconf.h
index de260ffbb4..5ccb879f2b 100644
--- a/firmware/config/stm32f4ems/chconf.h
+++ b/firmware/config/stm32f4ems/chconf.h
@@ -184,7 +184,7 @@
  * @note    The default is @p TRUE.
  */
 #if !defined(CH_USE_WAITEXIT) || defined(__DOXYGEN__)
-#define CH_USE_WAITEXIT                 TRUE
+#define CH_USE_WAITEXIT                 FALSE
 #endif
 
 /**
@@ -218,7 +218,7 @@
  * @note    Requires @p CH_USE_SEMAPHORES.
  */
 #if !defined(CH_USE_SEMSW) || defined(__DOXYGEN__)
-#define CH_USE_SEMSW                    TRUE
+#define CH_USE_SEMSW                    FALSE
 #endif
 
 /**
@@ -240,7 +240,7 @@
  * @note    Requires @p CH_USE_MUTEXES.
  */
 #if !defined(CH_USE_CONDVARS) || defined(__DOXYGEN__)
-#define CH_USE_CONDVARS                 TRUE
+#define CH_USE_CONDVARS                 FALSE
 #endif
 
 /**
@@ -252,7 +252,7 @@
  * @note    Requires @p CH_USE_CONDVARS.
  */
 #if !defined(CH_USE_CONDVARS_TIMEOUT) || defined(__DOXYGEN__)
-#define CH_USE_CONDVARS_TIMEOUT         TRUE
+#define CH_USE_CONDVARS_TIMEOUT         FALSE
 #endif
 
 /**
@@ -382,7 +382,7 @@
  * @note    Requires @p CH_USE_HEAP and/or @p CH_USE_MEMPOOLS.
  */
 #if !defined(CH_USE_DYNAMIC) || defined(__DOXYGEN__)
-#define CH_USE_DYNAMIC                  TRUE
+#define CH_USE_DYNAMIC                  FALSE
 #endif
 
 /** @} */
diff --git a/firmware/console/eficonsole.cpp b/firmware/console/eficonsole.cpp
index ceca50c657..a1ef353ed6 100644
--- a/firmware/console/eficonsole.cpp
+++ b/firmware/console/eficonsole.cpp
@@ -135,7 +135,7 @@ static void cmd_threads(void) {
 	tp = chRegFirstThread();
 	do {
 		print("%.8lx [%.8lx] %4lu %4lu %9s %lu %s\r\n", (uint32_t) tp, 0, (uint32_t) tp->p_prio,
-				(uint32_t) (tp->p_refs - 1), states[tp->p_state], (uint32_t) tp->p_time, tp->p_name);
+				(uint32_t) (0), states[tp->p_state], (uint32_t) tp->p_time, tp->p_name);
 		tp = chRegNextThread(tp);
 	} while (tp != NULL);
 #endif