diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp
index 57fd08db28..023c642cc3 100644
--- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp
+++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp
@@ -20,10 +20,12 @@ static const uavcan::int16_t ErrNotImplemented          = 1001; ///< Feature not
 static const uavcan::int16_t ErrInvalidBitRate          = 1002; ///< Bit rate not supported
 static const uavcan::int16_t ErrLogic                   = 1003; ///< Internal logic error
 static const uavcan::int16_t ErrUnsupportedFrame        = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
-static const uavcan::int16_t ErrMsrInakNotSet           = 1005; ///< INAK bit of the MSR register is not 1
-static const uavcan::int16_t ErrMsrInakNotCleared       = 1006; ///< INAK bit of the MSR register is not 0
+static const uavcan::int16_t ErrCCCrCSANotSet           = 1005; ///< CSA bit of the CCCR register is not 1
+static const uavcan::int16_t ErrCCCrCSANotCleared       = 1006; ///< CSA bit of the CCCR register is not 0
 static const uavcan::int16_t ErrBitRateNotDetected      = 1007; ///< Auto bit rate detection could not be finished
 static const uavcan::int16_t ErrFilterNumConfigs        = 1008; ///< Number of filters is more than supported
+static const uavcan::int16_t ErrCCCrINITNotSet          = 1000; ///< INIT bit of the CCCR register is not 1
+static const uavcan::int16_t ErrCCCrINITNotCleared      = 1010; ///< INIT bit of the CCCR register is not 0
 
 /**
  * RX queue item.
@@ -139,6 +141,8 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
 
 	virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
 
+	bool waitCCCRBitStateChange(uint32_t mask, bool target_state);
+
 public:
 	enum { MaxRxQueueCapacity = 64 };
 
diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp
index fdae6246e5..06da5292ee 100644
--- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp
+++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp
@@ -522,6 +522,29 @@ uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter
 	return 0;
 }
 
+bool CanIface::waitCCCRBitStateChange(uint32_t mask, bool target_state)
+{
+#if UAVCAN_STM32_NUTTX
+	const unsigned Timeout = 1000;
+#else
+	const unsigned Timeout = 2000000;
+#endif
+
+	for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) {
+		const bool state = (can_->CCCR & mask) != 0;
+
+		if (state == target_state) {
+			return true;
+		}
+
+#if UAVCAN_STM32_NUTTX
+		::usleep(1000);
+#endif
+	}
+
+	return false;
+}
+
 int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 {
 	/*
@@ -533,13 +556,20 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 		// Exit Power-down / Sleep mode, then wait for acknowledgement
 		can_->CCCR &= ~FDCAN_CCCR_CSR;
 
-		// TODO: add timeout
-		while ((can_->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA) {}
+		if (!waitCCCRBitStateChange(FDCAN_CCCR_CSA, false)) {
+			UAVCAN_STM32H7_LOG("CCCR CCCR_CSA not cleared");
+			can_->CCCR |= FDCAN_CCCR_INIT;
+			return -ErrCCCrCSANotCleared;
+		}
+
 
 		// Request Init mode, then wait for completion
 		can_->CCCR |= FDCAN_CCCR_INIT;
 
-		while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {}
+		if (!waitCCCRBitStateChange(FDCAN_CCCR_INIT, true)) {
+			UAVCAN_STM32H7_LOG("CCCR FDCAN_CCCR_INIT not set");
+			return -ErrCCCrINITNotSet;
+		}
 
 		// Configuration Changes Enable.  Can only be set during Init mode;
 		// cleared when INIT bit is cleared.
@@ -547,7 +577,8 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 
 		// Disable interrupts while we configure the hardware
 		can_->IE = 0;
-	}
+
+	} // End Critcal section
 
 	/*
 	 * Object state - interrupts are disabled, so it's safe to modify it now