diff --git a/stm32h7/devicetest/GyroL3GD20H.cpp b/stm32h7/devicetest/GyroL3GD20H.cpp
index 6f72ca7..a56ea4c 100644
--- a/stm32h7/devicetest/GyroL3GD20H.cpp
+++ b/stm32h7/devicetest/GyroL3GD20H.cpp
@@ -21,7 +21,7 @@ GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle): spiHandle(spiHandle) {
 ReturnValue_t GyroL3GD20H::initialize() {
     // Configure the SPI peripheral
     spiHandle->Instance               = SPI1;
-    spiHandle->Init.BaudRatePrescaler = SPI_GetPrescaler(HAL_RCC_GetHCLKFreq(), 3900000);
+    spiHandle->Init.BaudRatePrescaler = spi::getPrescaler(HAL_RCC_GetHCLKFreq(), 3900000);
     spiHandle->Init.Direction         = SPI_DIRECTION_2LINES;
     spi::assignSpiMode(spi::SpiModes::MODE_3, spiHandle);
     spiHandle->Init.DataSize          = SPI_DATASIZE_8BIT;
diff --git a/stm32h7/spi/spiDefinitions.cpp b/stm32h7/spi/spiDefinitions.cpp
index 9c141e7..4846293 100644
--- a/stm32h7/spi/spiDefinitions.cpp
+++ b/stm32h7/spi/spiDefinitions.cpp
@@ -24,3 +24,29 @@ void spi::assignSpiMode(SpiModes spiMode, SPI_HandleTypeDef *spiHandle) {
     }
     }
 }
+
+uint32_t spi::getPrescaler(uint32_t clock_src_freq, uint32_t baudrate_mbps) {
+    uint32_t divisor = 0;
+    uint32_t spi_clk = clock_src_freq;
+    uint32_t presc = 0;
+    static const uint32_t baudrate[] = {
+            SPI_BAUDRATEPRESCALER_2,
+            SPI_BAUDRATEPRESCALER_4,
+            SPI_BAUDRATEPRESCALER_8,
+            SPI_BAUDRATEPRESCALER_16,
+            SPI_BAUDRATEPRESCALER_32,
+            SPI_BAUDRATEPRESCALER_64,
+            SPI_BAUDRATEPRESCALER_128,
+            SPI_BAUDRATEPRESCALER_256,
+    };
+
+    while( spi_clk > baudrate_mbps) {
+        presc = baudrate[divisor];
+        if (++divisor > 7)
+            break;
+
+        spi_clk = ( spi_clk >> 1);
+    }
+
+    return presc;
+}
diff --git a/stm32h7/spi/spiDefinitions.h b/stm32h7/spi/spiDefinitions.h
index 3e002f2..ae07c7c 100644
--- a/stm32h7/spi/spiDefinitions.h
+++ b/stm32h7/spi/spiDefinitions.h
@@ -10,6 +10,14 @@ namespace spi {
 
 void assignSpiMode(SpiModes spiMode, SPI_HandleTypeDef* spiHandle);
 
+/**
+ * @brief  Set SPI frequency to calculate correspondent baud-rate prescaler.
+ * @param  clock_src_freq  Frequency of clock source
+ * @param  baudrate_mbps Baudrate to set to set
+ * @retval Baudrate prescaler
+ */
+uint32_t getPrescaler(uint32_t clock_src_freq, uint32_t baudrate_mbps);
+
 }