diff --git a/firmware/chibios/os/hal/platforms/STM32F4xx/hal_lld.h b/firmware/chibios/os/hal/platforms/STM32F4xx/hal_lld.h
index 0a0f94ab47..4facd4db65 100644
--- a/firmware/chibios/os/hal/platforms/STM32F4xx/hal_lld.h
+++ b/firmware/chibios/os/hal/platforms/STM32F4xx/hal_lld.h
@@ -401,7 +401,7 @@
 #define STM32_CAN_MAX_FILTERS   28
 
 /* DAC attributes.*/
-#define STM32_HAS_DAC           TRUE
+#define STM32_HAS_DAC           FALSE
 
 /* DMA attributes.*/
 #define STM32_ADVANCED_DMA      TRUE
@@ -409,7 +409,11 @@
 #define STM32_HAS_DMA2          TRUE
 
 /* ETH attributes.*/
+#if !defined(STM32F401xx)
 #define STM32_HAS_ETH           TRUE
+#else /* defined(STM32F401xx) */
+#define STM32_HAS_ETH           FALSE
+#endif /* defined(STM32F401xx) */
 
 /* EXTI attributes.*/
 #define STM32_EXTI_NUM_CHANNELS 23
@@ -425,11 +429,11 @@
 #define STM32_HAS_GPIOF         TRUE
 #define STM32_HAS_GPIOG         TRUE
 #define STM32_HAS_GPIOI         TRUE
-#else
+#else /* defined(STM32F401xx) */
 #define STM32_HAS_GPIOF         FALSE
 #define STM32_HAS_GPIOG         FALSE
 #define STM32_HAS_GPIOI         FALSE
-#endif
+#endif /* defined(STM32F401xx) */
 
 /* I2C attributes.*/
 #define STM32_HAS_I2C1          TRUE
@@ -491,7 +495,8 @@
                                  STM32_DMA_STREAM_ID_MSK(1, 7))
 #define STM32_SPI3_TX_DMA_CHN   0x00000000
 
-#if defined(STM32F427_437xx) || defined(STM32F429_439xx)
+#if defined(STM32F427_437xx) || defined(STM32F429_439xx) ||                 \
+    defined(STM32F401xx)
 #define STM32_HAS_SPI4          TRUE
 #define STM32_SPI4_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(2, 0) |            \
                                  STM32_DMA_STREAM_ID_MSK(2, 3))
@@ -499,7 +504,11 @@
 #define STM32_SPI4_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(2, 1) |            \
                                  STM32_DMA_STREAM_ID_MSK(2, 4))
 #define STM32_SPI4_TX_DMA_CHN   0x00050040
+#else
+#define STM32_HAS_SPI4          FALSE
+#endif
 
+#if defined(STM32F427_437xx) || defined(STM32F429_439xx)
 #define STM32_HAS_SPI5          TRUE
 #define STM32_SPI5_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(2, 3) |            \
                                  STM32_DMA_STREAM_ID_MSK(2, 5))
@@ -515,7 +524,6 @@
 #define STM32_SPI6_TX_DMA_CHN   0x00100000
 
 #else /* !(defined(STM32F427_437xx) || defined(STM32F429_439xx)) */
-#define STM32_HAS_SPI4          FALSE
 #define STM32_HAS_SPI5          FALSE
 #define STM32_HAS_SPI6          FALSE
 #endif /* !(defined(STM32F427_437xx) || defined(STM32F429_439xx)) */
@@ -526,15 +534,27 @@
 #define STM32_HAS_TIM3          TRUE
 #define STM32_HAS_TIM4          TRUE
 #define STM32_HAS_TIM5          TRUE
+#if !defined(STM32F401xx)
 #define STM32_HAS_TIM6          TRUE
 #define STM32_HAS_TIM7          TRUE
 #define STM32_HAS_TIM8          TRUE
+#else /* defined(STM32F401xx) */
+#define STM32_HAS_TIM6          FALSE
+#define STM32_HAS_TIM7          FALSE
+#define STM32_HAS_TIM8          FALSE
+#endif /* defined(STM32F401xx) */
 #define STM32_HAS_TIM9          TRUE
 #define STM32_HAS_TIM10         TRUE
 #define STM32_HAS_TIM11         TRUE
+#if !defined(STM32F401xx)
 #define STM32_HAS_TIM12         TRUE
 #define STM32_HAS_TIM13         TRUE
 #define STM32_HAS_TIM14         TRUE
+#else /* defined(STM32F401xx) */
+#define STM32_HAS_TIM12         FALSE
+#define STM32_HAS_TIM13         FALSE
+#define STM32_HAS_TIM14         FALSE
+#endif /* defined(STM32F401xx) */
 #define STM32_HAS_TIM15         FALSE
 #define STM32_HAS_TIM16         FALSE
 #define STM32_HAS_TIM17         FALSE
@@ -555,6 +575,7 @@
 #define STM32_USART2_TX_DMA_MSK (STM32_DMA_STREAM_ID_MSK(1, 6))
 #define STM32_USART2_TX_DMA_CHN 0x04000000
 
+#if !defined(STM32F401xx)
 #define STM32_HAS_USART3        TRUE
 #define STM32_USART3_RX_DMA_MSK (STM32_DMA_STREAM_ID_MSK(1, 1))
 #define STM32_USART3_RX_DMA_CHN 0x00000040
@@ -574,6 +595,12 @@
 #define STM32_UART5_TX_DMA_MSK  (STM32_DMA_STREAM_ID_MSK(1, 7))
 #define STM32_UART5_TX_DMA_CHN  0x40000000
 
+#else /* defined(STM32F401xx) */
+#define STM32_HAS_USART3        FALSE
+#define STM32_HAS_UART4         FALSE
+#define STM32_HAS_UART5         FALSE
+#endif /* defined(STM32F401xx) */
+
 #define STM32_HAS_USART6        TRUE
 #define STM32_USART6_RX_DMA_MSK (STM32_DMA_STREAM_ID_MSK(2, 1) |            \
                                  STM32_DMA_STREAM_ID_MSK(2, 2))
@@ -585,7 +612,11 @@
 /* USB attributes.*/
 #define STM32_HAS_USB           FALSE
 #define STM32_HAS_OTG1          TRUE
+#if !defined(STM32F401xx)
 #define STM32_HAS_OTG2          TRUE
+#else /* defined(STM32F401xx) */
+#define STM32_HAS_OTG2          FALSE
+#endif /* defined(STM32F401xx) */
 /** @} */
 
 /*===========================================================================*/
diff --git a/firmware/chibios/os/hal/platforms/STM32F4xx/stm32_isr.h b/firmware/chibios/os/hal/platforms/STM32F4xx/stm32_isr.h
index ade9b1094c..e8f45b0acc 100644
--- a/firmware/chibios/os/hal/platforms/STM32F4xx/stm32_isr.h
+++ b/firmware/chibios/os/hal/platforms/STM32F4xx/stm32_isr.h
@@ -36,23 +36,41 @@
 /*
  * CAN units.
  */
-#define STM32_CAN1_TX_HANDLER       CAN1_TX_IRQHandler
-#define STM32_CAN1_RX0_HANDLER      CAN1_RX0_IRQHandler
-#define STM32_CAN1_RX1_HANDLER      CAN1_RX1_IRQHandler
-#define STM32_CAN1_SCE_HANDLER      CAN1_SCE_IRQHandler
-#define STM32_CAN2_TX_HANDLER       CAN2_TX_IRQHandler
-#define STM32_CAN2_RX0_HANDLER      CAN2_RX0_IRQHandler
-#define STM32_CAN2_RX1_HANDLER      CAN2_RX1_IRQHandler
-#define STM32_CAN2_SCE_HANDLER      CAN2_SCE_IRQHandler
+#define STM32_CAN1_TX_HANDLER       Vector8C
+#define STM32_CAN1_RX0_HANDLER      Vector90
+#define STM32_CAN1_RX1_HANDLER      Vector94
+#define STM32_CAN1_SCE_HANDLER      Vector98
+#define STM32_CAN2_TX_HANDLER       Vector13C
+#define STM32_CAN2_RX0_HANDLER      Vector140
+#define STM32_CAN2_RX1_HANDLER      Vector144
+#define STM32_CAN2_SCE_HANDLER      Vector148
 
-#define STM32_CAN1_TX_NUMBER        CAN1_TX_IRQn
-#define STM32_CAN1_RX0_NUMBER       CAN1_RX0_IRQn
-#define STM32_CAN1_RX1_NUMBER       CAN1_RX1_IRQn
-#define STM32_CAN1_SCE_NUMBER       CAN1_SCE_IRQn
-#define STM32_CAN2_TX_NUMBER        CAN2_TX_IRQn
-#define STM32_CAN2_RX0_NUMBER       CAN2_RX0_IRQn
-#define STM32_CAN2_RX1_NUMBER       CAN2_RX1_IRQn
-#define STM32_CAN2_SCE_NUMBER       CAN2_SCE_IRQn
+#define STM32_CAN1_TX_NUMBER        19
+#define STM32_CAN1_RX0_NUMBER       20
+#define STM32_CAN1_RX1_NUMBER       21
+#define STM32_CAN1_SCE_NUMBER       22
+#define STM32_CAN2_TX_NUMBER        63
+#define STM32_CAN2_RX0_NUMBER       64
+#define STM32_CAN2_RX1_NUMBER       65
+#define STM32_CAN2_SCE_NUMBER       66
+
+/*
+ * I2C units.
+ */
+#define STM32_I2C1_EVENT_HANDLER    VectorBC
+#define STM32_I2C1_ERROR_HANDLER    VectorC0
+#define STM32_I2C1_EVENT_NUMBER     31
+#define STM32_I2C1_ERROR_NUMBER     32
+
+#define STM32_I2C2_EVENT_HANDLER    VectorC4
+#define STM32_I2C2_ERROR_HANDLER    VectorC8
+#define STM32_I2C2_EVENT_NUMBER     33
+#define STM32_I2C2_ERROR_NUMBER     34
+
+#define STM32_I2C3_EVENT_HANDLER    Vector160
+#define STM32_I2C3_ERROR_HANDLER    Vector164
+#define STM32_I2C3_EVENT_NUMBER     72
+#define STM32_I2C3_ERROR_NUMBER     73
 
 /*
  * OTG units.
@@ -62,67 +80,73 @@
 #define STM32_OTG2_EP1OUT_HANDLER   Vector168
 #define STM32_OTG2_EP1IN_HANDLER    Vector16C
 
-#define STM32_OTG1_NUMBER           OTG_FS_IRQn
-#define STM32_OTG2_NUMBER           OTG_HS_IRQn
-#define STM32_OTG2_EP1OUT_NUMBER    OTG_HS_EP1_OUT_IRQn
-#define STM32_OTG2_EP1IN_NUMBER     OTG_HS_EP1_IN_IRQn
+#define STM32_OTG1_NUMBER           67
+#define STM32_OTG2_NUMBER           77
+#define STM32_OTG2_EP1OUT_NUMBER    74
+#define STM32_OTG2_EP1IN_NUMBER     75
 
 /*
  * SDIO unit.
  */
-#define STM32_SDIO_HANDLER          SDIO_IRQHandler
+#define STM32_SDIO_HANDLER          Vector104
 
-#define STM32_SDIO_NUMBER           SDIO_IRQn
+#define STM32_SDIO_NUMBER           49
 
 /*
  * TIM units.
  */
-#define STM32_TIM1_UP_HANDLER       TIM1_UP_IRQHandler
-#define STM32_TIM1_CC_HANDLER       TIM1_CC_IRQHandler
-#define STM32_TIM2_HANDLER          TIM2_IRQHandler
-#define STM32_TIM3_HANDLER          TIM3_IRQHandler
-#define STM32_TIM4_HANDLER          TIM4_IRQHandler
-#define STM32_TIM5_HANDLER          TIM5_IRQHandler
-#define STM32_TIM6_HANDLER          TIM6_IRQHandler
-#define STM32_TIM7_HANDLER          TIM7_IRQHandler
-#define STM32_TIM8_UP_HANDLER       TIM8_UP_IRQHandler
-#define STM32_TIM8_CC_HANDLER       TIM8_CC_IRQHandler
-#define STM32_TIM9_HANDLER          TIM1_BRK_IRQHandler
-#define STM32_TIM11_HANDLER         TIM1_TRG_COM_IRQHandler
-#define STM32_TIM12_HANDLER         TIM8_BRK_IRQHandler
-#define STM32_TIM14_HANDLER         TIM8_TRG_COM_IRQHandler
+#define STM32_TIM1_UP_HANDLER       VectorA4
+#define STM32_TIM1_CC_HANDLER       VectorAC
+#define STM32_TIM2_HANDLER          VectorB0
+#define STM32_TIM3_HANDLER          VectorB4
+#define STM32_TIM4_HANDLER          VectorB8
+#define STM32_TIM5_HANDLER          Vector108
+#define STM32_TIM6_HANDLER          Vector118
+#define STM32_TIM7_HANDLER          Vector11C
+#define STM32_TIM8_UP_HANDLER       VectorF0
+#define STM32_TIM8_CC_HANDLER       VectorF8
+#define STM32_TIM9_HANDLER          VectorA0
+#define STM32_TIM11_HANDLER         VectorA8
+#define STM32_TIM12_HANDLER         VectorEC
+#define STM32_TIM14_HANDLER         VectorF4
 
-#define STM32_TIM1_UP_NUMBER        TIM1_UP_TIM10_IRQn
-#define STM32_TIM1_CC_NUMBER        TIM1_CC_IRQn
-#define STM32_TIM2_NUMBER           TIM2_IRQn
-#define STM32_TIM3_NUMBER           TIM3_IRQn
-#define STM32_TIM4_NUMBER           TIM4_IRQn
-#define STM32_TIM5_NUMBER           TIM5_IRQn
-#define STM32_TIM6_NUMBER           TIM6_IRQn
-#define STM32_TIM7_NUMBER           TIM7_IRQn
-#define STM32_TIM8_UP_NUMBER        TIM8_UP_TIM13_IRQn
-#define STM32_TIM8_CC_NUMBER        TIM8_CC_IRQn
-#define STM32_TIM9_NUMBER           TIM1_BRK_TIM9_IRQn
-#define STM32_TIM11_NUMBER          TIM1_TRG_COM_TIM11_IRQn
-#define STM32_TIM12_NUMBER          TIM8_BRK_TIM12_IRQn
-#define STM32_TIM14_NUMBER          TIM8_TRG_COM_TIM14_IRQn
+#define STM32_TIM1_UP_NUMBER        25
+#define STM32_TIM1_CC_NUMBER        27
+#define STM32_TIM2_NUMBER           28
+#define STM32_TIM3_NUMBER           29
+#define STM32_TIM4_NUMBER           30
+#define STM32_TIM5_NUMBER           50
+#define STM32_TIM6_NUMBER           54
+#define STM32_TIM7_NUMBER           55
+#define STM32_TIM8_UP_NUMBER        44
+#define STM32_TIM8_CC_NUMBER        46
+#define STM32_TIM9_NUMBER           24
+#define STM32_TIM11_NUMBER          26
+#define STM32_TIM12_NUMBER          43
+#define STM32_TIM14_NUMBER          45
 
 /*
  * USART units.
  */
-#define STM32_USART1_HANDLER        USART1_IRQHandler
-#define STM32_USART2_HANDLER        USART2_IRQHandler
-#define STM32_USART3_HANDLER        USART3_IRQHandler
-#define STM32_UART4_HANDLER         UART4_IRQHandler
-#define STM32_UART5_HANDLER         UART5_IRQHandler
-#define STM32_USART6_HANDLER        USART6_IRQHandler
+#define STM32_USART1_HANDLER        VectorD4
+#define STM32_USART2_HANDLER        VectorD8
+#define STM32_USART3_HANDLER        VectorDC
+#define STM32_UART4_HANDLER         Vector110
+#define STM32_UART5_HANDLER         Vector114
+#define STM32_USART6_HANDLER        Vector15C
+
+#define STM32_USART1_NUMBER         37
+#define STM32_USART2_NUMBER         38
+#define STM32_USART3_NUMBER         39
+#define STM32_UART4_NUMBER          52
+#define STM32_UART5_NUMBER          53
+#define STM32_USART6_NUMBER         71
+
+/*
+ * Ethernet
+ */
+#define ETH_IRQHandler              Vector134
 
-#define STM32_USART1_NUMBER         USART1_IRQn
-#define STM32_USART2_NUMBER         USART2_IRQn
-#define STM32_USART3_NUMBER         USART3_IRQn
-#define STM32_UART4_NUMBER          UART4_IRQn
-#define STM32_UART5_NUMBER          UART5_IRQn
-#define STM32_USART6_NUMBER         USART6_IRQn
 /** @} */
 
 /*===========================================================================*/
diff --git a/firmware/chibios/os/kernel/include/ch.h b/firmware/chibios/os/kernel/include/ch.h
index 2de6a11429..77a30d1241 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.3"
+#define CH_KERNEL_VERSION       "2.6.6"
 
 /**
  * @name    Kernel version
@@ -66,7 +66,7 @@
 /**
  * @brief   Kernel version patch number.
  */
-#define CH_KERNEL_PATCH         3
+#define CH_KERNEL_PATCH         6
 /** @} */
 
 /**
diff --git a/firmware/chibios/os/ports/GCC/ARMCMx/STM32F4xx/vectors.c b/firmware/chibios/os/ports/GCC/ARMCMx/STM32F4xx/vectors.c
index 3276989645..e3367f0995 100644
--- a/firmware/chibios/os/ports/GCC/ARMCMx/STM32F4xx/vectors.c
+++ b/firmware/chibios/os/ports/GCC/ARMCMx/STM32F4xx/vectors.c
@@ -62,7 +62,7 @@ typedef struct {
   irq_vector_t  vector34;
   irq_vector_t  pendsv_vector;
   irq_vector_t  systick_vector;
-  irq_vector_t  vectors[82];
+  irq_vector_t  vectors[91];
 } vectors_t;
 
 #if !defined(__DOXYGEN__)
@@ -164,6 +164,15 @@ extern void Vector178(void);
 extern void Vector17C(void);
 extern void Vector180(void);
 extern void Vector184(void);
+extern void Vector188(void);
+extern void Vector18C(void);
+extern void Vector190(void);
+extern void Vector194(void);
+extern void Vector198(void);
+extern void Vector19C(void);
+extern void Vector1A0(void);
+extern void Vector1A4(void);
+extern void Vector1A8(void);
 #endif
 
 /**
@@ -198,7 +207,9 @@ vectors_t _vectors = {
     Vector150,          Vector154,          Vector158,          Vector15C,
     Vector160,          Vector164,          Vector168,          Vector16C,
     Vector170,          Vector174,          Vector178,          Vector17C,
-    Vector180,          Vector184
+    Vector180,          Vector184,          Vector188,          Vector18C,
+    Vector190,          Vector194,          Vector198,          Vector19C,
+    Vector1A0,          Vector1A4,          Vector1A8
   }
 };
 
@@ -314,5 +325,14 @@ void Vector178(void) __attribute__((weak, alias("_unhandled_exception")));
 void Vector17C(void) __attribute__((weak, alias("_unhandled_exception")));
 void Vector180(void) __attribute__((weak, alias("_unhandled_exception")));
 void Vector184(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector188(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector18C(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector190(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector194(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector198(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector19C(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector1A0(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector1A4(void) __attribute__((weak, alias("_unhandled_exception")));
+void Vector1A8(void) __attribute__((weak, alias("_unhandled_exception")));
 
 /** @} */
diff --git a/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.c b/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.c
index 9d81ccb607..ce7d5f34b7 100644
--- a/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.c
+++ b/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.c
@@ -39,21 +39,6 @@
 /* Port interrupt handlers.                                                  */
 /*===========================================================================*/
 
-int getRemainingStack(Thread *otp) {
-#if CH_DBG_ENABLE_STACK_CHECK || defined(__DOXYGEN__)
-  int remainingStack;
-  if (dbg_isr_cnt > 0) {
-    remainingStack = 999; // todo
-  } else {
-    remainingStack = (stkalign_t *)(__get_SP() - sizeof(struct intctx)) - otp->p_stklimit;
-  }
-  otp->remainingStack = remainingStack;
-  return remainingStack;
-#else
-  return 999999;
-#endif  
-}
-
 /**
  * @brief   System Timer vector.
  * @details This interrupt is used as system tick.
@@ -80,6 +65,11 @@ CH_IRQ_HANDLER(SysTickVector) {
 void SVCallVector(void) {
   struct extctx *ctxp;
 
+#if CORTEX_USE_FPU
+  /* Enforcing unstacking of the FP part of the context.*/
+  SCB_FPCCR &= ~FPCCR_LSPACT;
+#endif
+
   /* Current PSP value.*/
   ctxp = (struct extctx *)__get_PSP();
 
@@ -87,11 +77,7 @@ void SVCallVector(void) {
      point to the real one.*/
   ctxp++;
 
-#if CORTEX_USE_FPU
-  /* Restoring the special register SCB_FPCCR.*/
-  SCB_FPCCR = (uint32_t)ctxp->fpccr;
-  SCB_FPCAR = SCB_FPCAR + sizeof (struct extctx);
-#endif
+  /* Restoring real position of the original stack frame.*/
   __set_PSP((unsigned long)ctxp);
   port_unlock_from_isr();
 }
@@ -107,6 +93,11 @@ void SVCallVector(void) {
 void PendSVVector(void) {
   struct extctx *ctxp;
 
+#if CORTEX_USE_FPU
+  /* Enforcing unstacking of the FP part of the context.*/
+  SCB_FPCCR &= ~FPCCR_LSPACT;
+#endif
+
   /* Current PSP value.*/
   ctxp = (struct extctx *)__get_PSP();
 
@@ -114,11 +105,7 @@ void PendSVVector(void) {
      point to the real one.*/
   ctxp++;
 
-#if CORTEX_USE_FPU
-  /* Restoring the special register SCB_FPCCR.*/
-  SCB_FPCCR = (uint32_t)ctxp->fpccr;
-  SCB_FPCAR = SCB_FPCAR + sizeof (struct extctx);
-#endif
+  /* Restoring real position of the original stack frame.*/
   __set_PSP((unsigned long)ctxp);
 }
 #endif /* CORTEX_SIMPLIFIED_PRIORITY */
@@ -171,24 +158,29 @@ void _port_irq_epilogue(void) {
   if ((SCB_ICSR & ICSR_RETTOBASE) != 0) {
     struct extctx *ctxp;
 
+#if CORTEX_USE_FPU
+    /* Enforcing a lazy FPU state save. Note, it goes in the original
+       context because the FPCAR register has not been modified.*/
+      (void)__get_FPSCR();
+#endif
+
     /* Current PSP value.*/
     ctxp = (struct extctx *)__get_PSP();
 
     /* Adding an artificial exception return context, there is no need to
        populate it fully.*/
     ctxp--;
-    __set_PSP((unsigned long)ctxp);
     ctxp->xpsr = (regarm_t)0x01000000;
+#if CORTEX_USE_FPU
+    ctxp->fpscr = (regarm_t)SCB_FPDSCR;
+#endif
+    __set_PSP((unsigned long)ctxp);
 
     /* The exit sequence is different depending on if a preemption is
        required or not.*/
     if (chSchIsPreemptionRequired()) {
       /* Preemption is required we need to enforce a context switch.*/
       ctxp->pc = (regarm_t)_port_switch_from_isr;
-#if CORTEX_USE_FPU
-      /* Triggering a lazy FPU state save.*/
-      (void)__get_FPSCR();
-#endif
     }
     else {
       /* Preemption not required, we just need to exit the exception
@@ -196,20 +188,6 @@ void _port_irq_epilogue(void) {
       ctxp->pc = (regarm_t)_port_exit_from_isr;
     }
 
-#if CORTEX_USE_FPU
-    {
-      uint32_t fpccr;
-
-      /* Saving the special register SCB_FPCCR into the reserved offset of
-         the Cortex-M4 exception frame.*/
-      (ctxp + 1)->fpccr = (regarm_t)(fpccr = SCB_FPCCR);
-
-      /* Now the FPCCR is modified in order to not restore the FPU status
-         from the artificial return context.*/
-      SCB_FPCCR = fpccr | FPCCR_LSPACT;
-    }
-#endif
-
     /* Note, returning without unlocking is intentional, this is done in
        order to keep the rest of the context switch atomic.*/
     return;
diff --git a/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.h b/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.h
index 71d5a8b017..6d3da5c4d7 100644
--- a/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.h
+++ b/firmware/chibios/os/ports/IAR/ARMCMx/chcore_v7m.h
@@ -267,7 +267,7 @@ struct extctx {
   regarm_t      s14;
   regarm_t      s15;
   regarm_t      fpscr;
-  regarm_t      fpccr;
+  regarm_t      reserved;
 #endif /* CORTEX_USE_FPU */
 };
 
diff --git a/firmware/hw_layer/stm32f4/mpu_util.cpp b/firmware/hw_layer/stm32f4/mpu_util.cpp
index 1537fde37b..61356ad5ff 100644
--- a/firmware/hw_layer/stm32f4/mpu_util.cpp
+++ b/firmware/hw_layer/stm32f4/mpu_util.cpp
@@ -15,7 +15,11 @@ int getRemainingStack(Thread *otp);
 
 extern stkalign_t __main_stack_base__;
 
+#if defined __GNUC__
+// GCC version
+
 int getRemainingStack(Thread *otp) {
+  
 #if CH_DBG_ENABLE_STACK_CHECK
 	register struct intctx *r13 asm ("r13");
 	otp->activeStack = r13;
@@ -35,6 +39,31 @@ int getRemainingStack(Thread *otp) {
 #endif /* CH_DBG_ENABLE_STACK_CHECK */
 }
 
+#else /* __GNUC__ */
+
+
+int getRemainingStack(Thread *otp) {
+#if CH_DBG_ENABLE_STACK_CHECK || defined(__DOXYGEN__)
+  int remainingStack;
+  if (dbg_isr_cnt > 0) {
+    remainingStack = 999; // todo
+  } else {
+    remainingStack = (stkalign_t *)(__get_SP() - sizeof(struct intctx)) - otp->p_stklimit;
+  }
+  otp->remainingStack = remainingStack;
+  return remainingStack;
+#else
+  return 999999;
+#endif  
+}
+
+// IAR version
+
+#endif
+
+
+
+
 
 void baseHardwareInit(void) {
 	// looks like this holds a random value on start? Let's set a nice clean zero