diff --git a/firmware/Makefile b/firmware/Makefile
index ba89cd40a7..a7b3d1f320 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -25,12 +25,12 @@ endif
 
 # Linker extra options here.
 ifeq ($(USE_LDOPT),)
-  USE_LDOPT = -flto=4
+  USE_LDOPT =
 endif
 
 # Enable this if you want link time optimizations (LTO)
 ifeq ($(USE_LTO),)
-  USE_LTO = yes
+  USE_LTO = no
 endif
 
 # If enabled, this option allows to compile the application in THUMB mode.
diff --git a/firmware/config/stm32f4ems/mcuconf.h b/firmware/config/stm32f4ems/mcuconf.h
index 3ef5b00faf..300751e786 100644
--- a/firmware/config/stm32f4ems/mcuconf.h
+++ b/firmware/config/stm32f4ems/mcuconf.h
@@ -46,7 +46,7 @@
 #define STM32_HSI_ENABLED                   TRUE
 #define STM32_LSI_ENABLED                   TRUE
 #define STM32_HSE_ENABLED                   TRUE
-#define STM32_LSE_ENABLED                   TRUE
+#define STM32_LSE_ENABLED                   FALSE
 #define STM32_CLOCK48_REQUIRED              TRUE
 #define STM32_SW                            STM32_SW_PLL
 #define STM32_PLLSRC                        STM32_PLLSRC_HSE
diff --git a/firmware/controllers/error_handling.cpp b/firmware/controllers/error_handling.cpp
index b58ed065a6..3bac9a451b 100644
--- a/firmware/controllers/error_handling.cpp
+++ b/firmware/controllers/error_handling.cpp
@@ -70,7 +70,7 @@ void chDbgPanic3(const char *msg, const char * file, int line) {
 
 	if (!main_loop_started) {
 		print("fatal %s %s:%d\r\n", msg, file, line);
-		chThdSleepSeconds(1);
+//		chThdSleepSeconds(1);
 		chSysHalt("Main loop did not start");
 	}
 }
diff --git a/firmware/hw_layer/can_hw.cpp b/firmware/hw_layer/can_hw.cpp
index b7f9607190..4bef1731d4 100644
--- a/firmware/hw_layer/can_hw.cpp
+++ b/firmware/hw_layer/can_hw.cpp
@@ -304,8 +304,8 @@ void initCan(void) {
 #else
 	canStart(&CAND1, &canConfig);
 #endif
-
-	canStart(&EFI_CAN_DEVICE, &canConfig);
+    // FIXME: Can't start a driver twice.
+    //canStart(&EFI_CAN_DEVICE, &canConfig);
 #if EFI_PROD_CODE || defined(__DOXYGEN__)
 
 	chThdCreateStatic(canTreadStack, sizeof(canTreadStack), NORMALPRIO, (tfunc_t) canThread, NULL);
diff --git a/firmware/hw_layer/serial_over_usb/usbcfg.c b/firmware/hw_layer/serial_over_usb/usbcfg.c
index c712b0bce5..262f3f6f2b 100644
--- a/firmware/hw_layer/serial_over_usb/usbcfg.c
+++ b/firmware/hw_layer/serial_over_usb/usbcfg.c
@@ -1,5 +1,5 @@
 /*
-    ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
 
     Licensed under the Apache License, Version 2.0 (the "License");
     you may not use this file except in compliance with the License.
@@ -14,9 +14,10 @@
     limitations under the License.
 */
 
-#include "main.h"
+#include "hal.h"
 
-#if HAL_USE_SERIAL_USB || defined(__DOXYGEN__)
+/* Virtual serial port over USB.*/
+SerialUSBDriver SDU1;
 
 /*
  * Endpoints to be used for USBD1.
@@ -287,6 +288,12 @@ static void usb_event(USBDriver *usbp, usbevent_t event) {
   case USB_EVENT_UNCONFIGURED:
     return;
   case USB_EVENT_SUSPEND:
+    chSysLockFromISR();
+
+    /* Disconnection event on suspend.*/
+    sduDisconnectI(&SDU1);
+
+    chSysUnlockFromISR();
     return;
   case USB_EVENT_WAKEUP:
     return;
@@ -296,6 +303,18 @@ static void usb_event(USBDriver *usbp, usbevent_t event) {
   return;
 }
 
+/*
+ * Handles the USB driver global events.
+ */
+static void sof_handler(USBDriver *usbp) {
+
+  (void)usbp;
+
+  osalSysLockFromISR();
+  sduSOFHookI(&SDU1);
+  osalSysUnlockFromISR();
+}
+
 /*
  * USB driver configuration.
  */
@@ -303,7 +322,7 @@ const USBConfig usbcfg = {
   usb_event,
   get_descriptor,
   sduRequestsHook,
-  NULL
+  sof_handler
 };
 
 /*
@@ -315,7 +334,3 @@ const SerialUSBConfig serusbcfg = {
   USBD1_DATA_AVAILABLE_EP,
   USBD1_INTERRUPT_REQUEST_EP
 };
-
-/* Virtual serial port over USB.*/
-SerialUSBDriver SDU1;
-#endif
diff --git a/firmware/hw_layer/stm32f4/mpu_util.cpp b/firmware/hw_layer/stm32f4/mpu_util.cpp
index 41d492708c..5e6ab5bd1a 100644
--- a/firmware/hw_layer/stm32f4/mpu_util.cpp
+++ b/firmware/hw_layer/stm32f4/mpu_util.cpp
@@ -38,7 +38,7 @@ int getRemainingStack(thread_t *otp) {
 	otp->activeStack = r13;
 
 	int remainingStack;
-	if (dbg_isr_cnt > 0) {
+    if (ch.dbg.isr_cnt > 0) {
 		// ISR context
 		remainingStack = (int)(r13 - 1) - (int)&__main_stack_base__;
 	} else {
diff --git a/firmware/hw_layer/trigger_input.cpp b/firmware/hw_layer/trigger_input.cpp
index 47b53b49d8..b9c5842eac 100644
--- a/firmware/hw_layer/trigger_input.cpp
+++ b/firmware/hw_layer/trigger_input.cpp
@@ -111,7 +111,7 @@ static ICUDriver *turnOnTriggerInputPin(const char *msg, brain_pin_e hwPin, ICUC
 
 		efiIcuStart(driver, icucfg);
 		if (driver->state == ICU_READY) {
-			icuEnableNotifications(driver);
+            icuStartCapture(driver);
 		} else {
 			// we would be here for example if same pin is used for multiple input capture purposes
 			firmwareError(CUSTOM_ERR_ICU_STATE, "ICU unexpected state [%s]", hwPortname(hwPin));
@@ -123,7 +123,7 @@ static ICUDriver *turnOnTriggerInputPin(const char *msg, brain_pin_e hwPin, ICUC
 static void turnOffTriggerInputPin(brain_pin_e hwPin) {
 	ICUDriver *driver = getInputCaptureDriver("trigger_off", hwPin);
 	if (driver != NULL) {
-		icuDisableNotificationsI(driver);
+        icuStopCapture(driver);
 		icuStop(driver);
 		scheduleMsg(logger, "turnOffTriggerInputPin %s", hwPortname(hwPin));
 		unmarkPin(hwPin);
diff --git a/firmware/util/rfiutil.h b/firmware/util/rfiutil.h
index 4b8eaf98ea..f9770f792d 100644
--- a/firmware/util/rfiutil.h
+++ b/firmware/util/rfiutil.h
@@ -15,16 +15,14 @@
 
 #include "histogram.h"
 
-extern cnt_t dbg_lock_cnt;
-extern cnt_t dbg_isr_cnt;
-#define isLocked() (dbg_lock_cnt > 0)
+#define isLocked() (ch.dbg.lock_cnt > 0)
 
 	/**
 	 * Unfortunately ChibiOS has two versions of methods for different
 	 * contexts.
 	 */
 
-#define isIsrContext() (dbg_isr_cnt > 0)
+#define isIsrContext() (ch.dbg.isr_cnt > 0)
 
 #ifdef __cplusplus
 extern "C"