diff --git a/README.md b/README.md
index 57a9791..f36ca0e 100644
--- a/README.md
+++ b/README.md
@@ -85,6 +85,12 @@ Configuration:
 TIM time base must be started before using `sta::delayUs` by calling `sta::initHAL`.
 When using the startup system task this is handled automatically.
 
+Steps to enable delayUs: 
+* Include sta/devices/stm32/delay.hpp in the file where the delayUs function is called
+* Enable a timer TIM in .ioc file with the settings: Slave Mode=Disable, Trigger Mode=Disable, Clock Source=Internal Clock, all other channels =Disable)
+* Define STA_STM32_DELAY_US_TIM in App/Inc/sta/config.hpp as the timer handle (can be found in Core/Inc/tim.h). For TIM1, this would be htim1
+ 
+
 
 ## Interfaces
 
diff --git a/include/sta/debug/assert.hpp b/include/sta/debug/assert.hpp
index f6e82a9..bd02810 100644
--- a/include/sta/debug/assert.hpp
+++ b/include/sta/debug/assert.hpp
@@ -3,9 +3,8 @@
  * @brief Assertion handling.
  *
  * Configuration:
- *   * STA_ASSERT_FORCE:    Ignore debug defines and always enable assertions
- *   * DEBUG:               Enables assertions when defined
- *   * NDEBUG:              Disables assertions when defined (overrides DEBUG)
+ *   * STA_ASSERT_ENABLED:	Enable assertions
+ *	 * STA_ASSERT_FORCE:	Enable assertions. Still there for backwards compatibility.
  */
 #ifndef STA_CORE_ASSERT_HPP
 #define STA_CORE_ASSERT_HPP
@@ -24,18 +23,10 @@
 
 #include <sta/config.hpp>
 
-// Determine if module should be enabled
-// Condition:
-//     STA_ASSERT_FORCE is defined
-//   or
-//     DEBUG is defined but not NDEBUG
+// Keep STA_ASSERT_FORCE for backwards comapatibility.
 #ifdef STA_ASSERT_FORCE
-#  define STA_ASSERT_ENABLED
-#else // !STA_ASSERT_FORCE
-#  if defined(DEBUG) && !defined(NDEBUG)
-#    define STA_ASSERT_ENABLED
-#  endif // DEBUG && !NDEBUG
-#endif // !STA_ASSERT_FORCE
+#	define STA_ASSERT_ENABLED
+#endif // STA_ASSERT_FORCE
 
 
 #if defined(STA_ASSERT_ENABLED) || defined(DOXYGEN)
diff --git a/include/sta/debug/profile.hpp b/include/sta/debug/profile.hpp
new file mode 100644
index 0000000..f256ac5
--- /dev/null
+++ b/include/sta/debug/profile.hpp
@@ -0,0 +1,47 @@
+/*
+ * profile.hpp
+ *
+ *  Created on: May 22, 2024
+ *      Author: Dario
+ */
+
+#ifndef STA_DEBUGGING_PROFILING_HPP
+#define STA_DEBUGGING_PROFILING_HPP
+
+#include <sta/debug/debug.hpp>
+
+#ifdef STA_PROFILING_ENABLED
+
+#ifndef STA_DEBUGGING_ENABLED
+#	error "Debugging has to be enabled in order to use profiling."
+#endif // STA_DEBUGGING_ENABLED
+
+#ifndef STA_STM32_DELAY_US_TIM
+#	error "A microsecond timer has to be defined in order to use profiling." 
+#endif // STA_STM32_DELAY_US_TIM
+
+namespace sta
+{
+	class Profiler {
+	public:
+		Profiler(const char* name);
+
+		~Profiler();
+	private:
+		const char* name_;
+		uint32_t start_;
+	};
+}  // namespace sta
+
+/**
+ *
+ */
+#define STA_TIME_IT(name)	sta::Profiler profiler(name);
+
+#else
+
+#define STA_TIME_IT(name)	((void)0)
+
+#endif // // STA_PROFILING_ENABLED
+
+#endif // STA_DEBUGGING_PROFILING_HPP
diff --git a/include/sta/devices/stm32/clocks.hpp b/include/sta/devices/stm32/clocks.hpp
index 0d8d832..bb1be59 100644
--- a/include/sta/devices/stm32/clocks.hpp
+++ b/include/sta/devices/stm32/clocks.hpp
@@ -39,7 +39,7 @@ namespace sta
  *
  * @param n Index of peripheral clock
  */
-#define STA_STM32_GET_PCLK_FREQ_FN(n)               HAL_RCC_GetPCLK ## n ## Freq
+#define STA_STM32_GET_PCLK_FREQ_FN(n)                   HAL_RCC_GetPCLK ## n ## Freq
 
 /**
  * @brief Internal helper for macro expansion.
@@ -47,7 +47,31 @@ namespace sta
  * @param n PCLK index
  * @return  Function returning PCLK frequency
  */
-#define _STA_STM32_GET_PCLK_FREQ_FN(n)              STA_STM32_GET_PCLK_FREQ_FN(n)
+#define _STA_STM32_GET_PCLK_FREQ_FN(n)                  STA_STM32_GET_PCLK_FREQ_FN(n)
+
+/**
+ * @brief Get the PCLK frequency.
+ * 
+ * @param n PCLK index
+ * @return  The PCLK frequency
+ */
+#define _STA_STM32_GET_PCLK_FREQ(n)                     STA_STM32_GET_PCLK_FREQ_FN(n)()
+
+/**
+ * @brief Gets the prescaler from APBx to APBx timer clocks.
+ * 
+ * @param n PCLK index
+ * @return  The prescaler from PCLK to TIM
+ */
+#define STA_STM32_GET_PCLK_TIM_PRESCALER(n)             STA_STM32_TIM_PCLK_ ## n ## ## _PRESCALER
+
+/**
+ * @brief Internal helper for macro expansion.
+ *
+ * @param n PCLK index
+ * @return  Function returning PCLK frequency
+ */
+#define _STA_STM32_GET_PCLK_TIM_PRESCALER(n)            STA_STM32_GET_PCLK_TIM_PRESCALER(n)
 
 /**
  * @brief Map instance name to PCLK index.
@@ -56,52 +80,67 @@ namespace sta
  * @param idx   Instance index
  * @return      PCLK index
  */
-#define _STA_STM32_PCLK_IDX_MAP(type, idx)          STA_STM32_ ## type ## _ ## idx ## _PCLK_IDX
+#define _STA_STM32_PCLK_IDX_MAP(type, idx)              STA_STM32_ ## type ## _ ## idx ## _PCLK_IDX
 // Get HAL handle to PCLK index map macro
+
 /**
  * @brief Map instance handle to PCLK index.
  *
  * @param handle    HAL handle
  * @return          PCLK index
  */
-#define _STA_STM32_HANDLE_PCLK_IDX_MAP(handle)      STA_STM32_ ## handle ## _PCLK_IDX
-
+#define _STA_STM32_HANDLE_PCLK_IDX_MAP(handle)          STA_STM32_ ## handle ## _PCLK_IDX
 
 /**
- * @brief Get function returning frequency of PCLK used by TIM.
+ * @brief Get a function returning the frequency of PCLK used by TIM.
  *
  * @param n         TIM index
  */
-#define STA_STM32_GET_TIM_PCLK_FREQ_FN(n)           _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(TIM, n))
+#define STA_STM32_GET_TIM_PCLK_FREQ_FN(n)               _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(TIM, n)) 
 
 /**
- * @brief Get function returning frequency of PCLK used by SPI interface.
+ * @brief Internal helper for macro expansion.
+ *
+ * @param n         TIM index 
+ */
+#define _STA_STM32_GET_TIM_PCLK_FREQ_FN(n)              STA_STM32_GET_TIM_PCLK_FREQ_FN(n)                        
+
+/**
+ * @brief Get a function returning the frequency of PCLK used by SPI interface.
  *
  * @param n         SPI interface index
  */
-#define STA_STM32_GET_SPI_PCLK_FREQ_FN(n)           _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(SPI, n))
+#define STA_STM32_GET_SPI_PCLK_FREQ_FN(n)               _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(SPI, n))
 
 /**
- * @brief Get function returning frequency of PCLK used by I2C interface.
+ * @brief Get a function returning the frequency of PCLK used by I2C interface.
  *
  * @param n         I2C interface index
  */
-#define STA_STM32_GET_I2C_PCLK_FREQ_FN(n)           _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(I2C, n))
+#define STA_STM32_GET_I2C_PCLK_FREQ_FN(n)               _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(I2C, n))
 
 /**
- * @brief Get function returning frequency of PCLK used by USART interface.
+ * @brief Get a function returning the frequency of PCLK used by USART interface.
  *
  * @param n         USART interface index
  */
-#define STA_STM32_GET_USART_PCLK_FREQ_FN(n)         _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(USART, n))
+#define STA_STM32_GET_USART_PCLK_FREQ_FN(n)             _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_PCLK_IDX_MAP(USART, n))
 
 
 /**
- * @brief Get function returning frequency of PCLK used by HAL instance.
+ * @brief Get a function returning the frequency of PCLK used by HAL instance.
  *
  * @param handle    Instance handle
  */
-#define STA_STM32_GET_HANDLE_PCLK_FREQ_FN(handle)   _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_HANDLE_PCLK_IDX_MAP(handle))
+#define STA_STM32_GET_HANDLE_PCLK_FREQ_FN(handle)       _STA_STM32_GET_PCLK_FREQ_FN(_STA_STM32_HANDLE_PCLK_IDX_MAP(handle))
+
+/**
+ * @brief Get the PCLK to timer prescaler for TIM given by the handle.
+ * 
+ * @param handle    TIM handle
+ * @return          PCLK-TIM prescaler
+ */
+#define STA_STM32_GET_HANDLE_PCLK_TIM_PRESCALER(handle) _STA_STM32_GET_PCLK_TIM_PRESCALER(_STA_STM32_HANDLE_PCLK_IDX_MAP(handle))
 
 
 /** @} */
diff --git a/include/sta/devices/stm32/init.hpp b/include/sta/devices/stm32/init.hpp
index 7fee75f..cd34558 100644
--- a/include/sta/devices/stm32/init.hpp
+++ b/include/sta/devices/stm32/init.hpp
@@ -5,6 +5,7 @@
 #ifndef STA_CORE_STM32_INIT_HPP
 #define STA_CORE_STM32_INIT_HPP
 
+#include <sta/devices/stm32/hal.hpp>
 
 namespace sta
 {
diff --git a/include/sta/devices/stm32/mcu/STM32F407xx.hpp b/include/sta/devices/stm32/mcu/STM32F407xx.hpp
index f390ad0..3a3b61d 100644
--- a/include/sta/devices/stm32/mcu/STM32F407xx.hpp
+++ b/include/sta/devices/stm32/mcu/STM32F407xx.hpp
@@ -64,6 +64,11 @@
 #define STA_STM32_USART_3_PCLK_IDX      1 /**< USART3 to PCLK index */
 #define STA_STM32_USART_6_PCLK_IDX      2 /**< USART6 to PCLK index */
 
+// prescaler from APBx to APBx timer clocks
+#define STA_STM32_TIM_PCLK_1_PRESCALER  2 /**< PCLK1 has prescaler of 2 */
+#define STA_STM32_TIM_PCLK_2_PRESCALER  2 /**< PCLK2 has prescaler of 1 */
+
+
 // HAL handle mappings
 //
 
diff --git a/include/sta/devices/stm32/mcu/STM32F411xE.hpp b/include/sta/devices/stm32/mcu/STM32F411xE.hpp
index ee91c61..56608f5 100644
--- a/include/sta/devices/stm32/mcu/STM32F411xE.hpp
+++ b/include/sta/devices/stm32/mcu/STM32F411xE.hpp
@@ -63,8 +63,13 @@
 #define STA_STM32_USART_2_PCLK_IDX      1 /**< USART2 to PCLK index 1 */
 #define STA_STM32_USART_6_PCLK_IDX      2 /**< USART6 to PCLK index 2 */
 
+// prescaler from APBx to APBx timer clocks
+#define STA_STM32_TIM_PCLK_1_PRESCALER  2 /**< PCLK1 has prescaler of 2 */    
+#define STA_STM32_TIM_PCLK_2_PRESCALER  1 /**< PCLK2 has prescaler of 1 */   
+
 
 // HAL handle mappings
+//
 
 // TIM to PCLK
 #define STA_STM32_htim1_PCLK_IDX        STA_STM32_TIM_1_PCLK_IDX /**< HAL TIM1 to PCLK index */
diff --git a/include/sta/mutex.hpp b/include/sta/mutex.hpp
index 1147f75..6dd2094 100644
--- a/include/sta/mutex.hpp
+++ b/include/sta/mutex.hpp
@@ -5,9 +5,16 @@
 #ifndef STA_CORE_MUTEX_HPP
 #define STA_CORE_MUTEX_HPP
 
+#include <mutex>
+
 
 namespace sta
 {
+	/**
+	 * @brief Hippity hoppity, this is now namespace sta's property.
+	 */
+	using std::lock_guard;
+
     /**
      * @brief Interface for mutex objects.
      *
@@ -25,6 +32,16 @@ namespace sta
          */
         virtual void release() = 0;
 
+        /**
+         * @brief Identical to acquire(). Added for compatibility with std::lock_guard.
+         */
+        void lock();
+
+        /**
+		 * @brief Identical to release(). Added for compatibility with std::lock_guard.
+		 */
+        void unlock();
+
         static Mutex * ALWAYS_FREE; /**< Fake mutex that can always be acquired */
     };
 } // namespace sta
diff --git a/include/sta/time.hpp b/include/sta/time.hpp
index 6495952..1cb5562 100644
--- a/include/sta/time.hpp
+++ b/include/sta/time.hpp
@@ -16,12 +16,27 @@ namespace sta
      * @return Time in milliseconds
      */
     using TimeMsFn = uint32_t (*)();
+
     /**
      * @brief Signature for microseconds precision time.
      *
      * @return Time in microseconds
      */
     using TimeUsFn = uint32_t (*)();
+
+    /**
+     * @brief Gets the current time in milliseconds.
+     *
+     * @return Time in milliseconds
+     */
+    uint32_t timeMs();
+
+    /**
+     * @brief Gets the current time in microseconds.
+     *
+     * @return Time in microseconds
+     */
+    uint32_t timeUs();
 } // namespace sta
 
 
diff --git a/src/debug/profile.cpp b/src/debug/profile.cpp
new file mode 100644
index 0000000..4e7eec6
--- /dev/null
+++ b/src/debug/profile.cpp
@@ -0,0 +1,29 @@
+/*
+ * profiler.cpp
+ *
+ *  Created on: May 22, 2024
+ *      Author: Dario
+ */
+
+#include <sta/debug/profile.hpp>
+
+#ifdef STA_PROFILING_ENABLED
+
+#include <sta/time.hpp>
+
+namespace sta
+{
+	Profiler::Profiler(const char* name)
+		: name_{name},
+		  start_{timeUs()}
+	{
+
+	}
+
+	Profiler::~Profiler()
+	{
+		STA_DEBUG_PRINTF("[PROFILER] %s took %d us", name_, timeUs() - start_);
+	}
+} // namespace sta
+
+#endif // STA_PROFILING_ENABLED
diff --git a/src/devices/stm32/delay.cpp b/src/devices/stm32/delay.cpp
index 8f51d73..3e754d5 100644
--- a/src/devices/stm32/delay.cpp
+++ b/src/devices/stm32/delay.cpp
@@ -51,7 +51,8 @@ namespace sta
 		}
 
 		// Calculate TIM clock frequency
-		uint32_t clkFreq = pclkMul * STA_STM32_GET_HANDLE_PCLK_FREQ_FN(STA_STM32_DELAY_US_TIM)();
+		uint32_t clkFreq = pclkMul * STA_STM32_GET_HANDLE_PCLK_FREQ_FN(STA_STM32_DELAY_US_TIM)() * STA_STM32_GET_HANDLE_PCLK_TIM_PRESCALER(STA_STM32_DELAY_US_TIM);
+
 		// Calculate update frequency based on prescaler value
 		uint32_t prescaler = (STA_STM32_DELAY_US_TIM.Init.Prescaler) ? STA_STM32_DELAY_US_TIM.Init.Prescaler : 1;
 		uint32_t updateFreq = clkFreq / prescaler;
@@ -67,13 +68,22 @@ namespace sta
     	// Check if the specified timer is usable for microsecond delays.
     	STA_ASSERT(isValidDelayUsTIM());
 
-        __HAL_TIM_SET_COUNTER(&STA_STM32_DELAY_US_TIM, 0);
-        while (__HAL_TIM_GET_COUNTER(&STA_STM32_DELAY_US_TIM) < us * gDelayUsMul);
+		// Save the current value of the counter.
+		uint32_t current = __HAL_TIM_GET_COUNTER(&STA_STM32_DELAY_US_TIM);
+
+		// Set the timer counter to zero to avoid overflows during delay-
+		__HAL_TIM_SET_COUNTER(&STA_STM32_DELAY_US_TIM, 0);
+
+		// Wait for the desired microseconds.
+		while (__HAL_TIM_GET_COUNTER(&STA_STM32_DELAY_US_TIM) < us * gDelayUsMul);
+
+		// Set the timer counter to the previous value and add the waited duration to it.
+		// This avoids collisions with code relying on timeUs measurements.
+		__HAL_TIM_SET_COUNTER(&STA_STM32_DELAY_US_TIM, current + us * gDelayUsMul);
     }
 
 } // namespace sta
 
 #endif // STA_STM32_DELAY_US_TIM
 
-
 #endif // STA_PLATFORM_STM32
diff --git a/src/devices/stm32/init.cpp b/src/devices/stm32/init.cpp
index 0f06016..dc07c79 100644
--- a/src/devices/stm32/init.cpp
+++ b/src/devices/stm32/init.cpp
@@ -23,6 +23,8 @@ namespace sta
         STA_ASSERT(isValidDelayUsTIM());
         // Start timer base
         HAL_TIM_Base_Start(&STA_STM32_DELAY_US_TIM);
+#else
+
 #endif // STA_STM32_DELAY_US_TIM
     }
 } // namespace sta
diff --git a/src/devices/stm32/time.cpp b/src/devices/stm32/time.cpp
new file mode 100644
index 0000000..97d5a1c
--- /dev/null
+++ b/src/devices/stm32/time.cpp
@@ -0,0 +1,33 @@
+/*
+ * time.cpp
+ *
+ *  Created on: May 22, 2024
+ *      Author: Dario
+ */
+
+#include <sta/time.hpp>
+
+#include <sta/config.hpp>
+#ifdef STA_PLATFORM_STM32
+
+#include <sta/devices/stm32/hal.hpp>
+#include <sta/devices/stm32/clocks.hpp>
+
+namespace sta
+{
+	uint32_t timeMs()
+	{
+		return HAL_GetTick();
+	}
+
+#ifdef STA_STM32_DELAY_US_TIM
+	extern uint32_t gDelayUsMul;
+
+	uint32_t timeUs()
+	{
+		return __HAL_TIM_GET_COUNTER(&STA_STM32_DELAY_US_TIM) / gDelayUsMul;
+	}
+}
+#endif // STA_STM32_DELAY_US_TIM
+
+#endif // STA_PLATFORM_STM32
diff --git a/src/mutex.cpp b/src/mutex.cpp
index 2472911..ea8438b 100644
--- a/src/mutex.cpp
+++ b/src/mutex.cpp
@@ -3,6 +3,16 @@
 
 namespace sta
 {
+	void Mutex::lock()
+	{
+		acquire();
+	}
+
+	void Mutex::unlock()
+	{
+		release();
+	}
+
     /**
      * @brief Dummy mutex implementation with no access control.
      */
diff --git a/src/time.cpp b/src/time.cpp
new file mode 100644
index 0000000..6e00ccf
--- /dev/null
+++ b/src/time.cpp
@@ -0,0 +1,30 @@
+/*
+ * time.cpp
+ *
+ *  Created on: May 22, 2024
+ *      Author: Dario
+ */
+
+#include <sta/time.hpp>
+#include <sta/lang.hpp>
+
+namespace sta
+{
+	STA_WEAK
+	uint32_t timeMs()
+	{
+		STA_NOT_IMPLEMENTED();
+
+		return 0;
+	}
+
+	STA_WEAK
+	uint32_t timeUs()
+	{
+		STA_NOT_IMPLEMENTED();
+
+		return 0;
+	}
+}  // namespace sta
+
+