diff --git a/src/rtos/ChibiOS.c b/src/rtos/ChibiOS.c
index be5fe8472..71f929fe7 100644
--- a/src/rtos/ChibiOS.c
+++ b/src/rtos/ChibiOS.c
@@ -29,6 +29,8 @@
 #include <jtag/jtag.h>
 #include "target/target.h"
 #include "target/target_type.h"
+#include "target/armv7m.h"
+#include "target/cortex_m.h"
 #include "rtos.h"
 #include "helper/log.h"
 #include "helper/types.h"
@@ -94,12 +96,12 @@ struct ChibiOS_params ChibiOS_params_list[] = {
 	{
 	"cortex_m3",							/* target_name */
 	0,
-	&rtos_chibios_arm_v7m_stacking,		/* stacking_info */
+	NULL,									/* stacking_info */
 	},
 	{
 	"stm32_stlink",							/* target_name */
 	0,
-	&rtos_chibios_arm_v7m_stacking,		/* stacking_info */
+	NULL,									/* stacking_info */
 	}
 };
 #define CHIBIOS_NUM_PARAMS ((int)(sizeof(ChibiOS_params_list)/sizeof(struct ChibiOS_params)))
@@ -219,8 +221,43 @@ static int ChibiOS_update_stacking(struct rtos *rtos)
 	 *    available than the current execution. In which case
 	 *    ChibiOS_get_thread_reg_list is called.
 	 */
+	int retval;
+
+	if (!rtos->rtos_specific_params)
+		return -1;
+
+	struct ChibiOS_params *param;
+	param = (struct ChibiOS_params *) rtos->rtos_specific_params;
+
+	/* Check for armv7m with *enabled* FPU, i.e. a Cortex M4  */
+	struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
+	if (is_armv7m(armv7m_target)) {
+		if (armv7m_target->fp_feature == FPv4_SP) {
+			/* Found ARM v7m target which includes a FPU */
+			uint32_t cpacr;
+
+			retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not read CPACR register to check FPU state");
+				return -1;
+			}
+
+			/* Check if CP10 and CP11 are set to full access.
+			 * In ChibiOS this is done in ResetHandler() in crt0.c */
+			if (cpacr & 0x00F00000) {
+				/* Found target with enabled FPU */
+				/* FIXME: Need to figure out how to specify the FPU registers */
+				LOG_ERROR("ChibiOS ARM v7m targets with enabled FPU "
+						  " are NOT supported");
+				return -1;
+			}
+		}
+
+		/* Found ARM v7m target with no or disabled FPU */
+		param->stacking_info = &rtos_chibios_arm_v7m_stacking;
+		return 0;
+	}
 
-	/* TODO: Add actual detection, currently it will not work  with FPU enabled.*/
 	return -1;
 }
 
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index a10368d52..743bd13db 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -59,6 +59,11 @@
 #define FP_COMP6	0xE0002020
 #define FP_COMP7	0xE0002024
 
+#define FPU_CPACR	0xE000ED88
+#define FPU_FPCCR	0xE000EF34
+#define FPU_FPCAR	0xE000EF38
+#define FPU_FPDSCR	0xE000EF3C
+
 /* DCB_DHCSR bit and field definitions */
 #define DBGKEY		(0xA05F << 16)
 #define C_DEBUGEN	(1 << 0)