diff --git a/stm32h7/devicetest/GyroL3GD20H.cpp b/stm32h7/devicetest/GyroL3GD20H.cpp
index 10e5335..736ec2f 100644
--- a/stm32h7/devicetest/GyroL3GD20H.cpp
+++ b/stm32h7/devicetest/GyroL3GD20H.cpp
@@ -1,6 +1,6 @@
 #include "GyroL3GD20H.h"
-#include "stm32h7xx_spi_dma_msp.h"
 #include "spiConf.h"
+#include "../spi/mspInit.h"
 #include "../spi/spiDefinitions.h"
 #include "../spi/spiCore.h"
 
@@ -9,6 +9,7 @@
 
 #include "stm32h7xx_nucleo.h"
 #include "stm32h7xx_hal_spi.h"
+#include "stm32h7xx_hal_rcc.h"
 
 std::array<uint8_t, GyroL3GD20H::recvBufferSize> GyroL3GD20H::rxBuffer;
 TransferStates transferState = TransferStates::IDLE;
@@ -16,18 +17,19 @@ DMA_HandleTypeDef txDmaHandle;
 DMA_HandleTypeDef rxDmaHandle;
 
 GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle): spiHandle(spiHandle) {
-    setDmaHandles(&txDmaHandle, &rxDmaHandle);
+    set_dma_handles(&txDmaHandle, &rxDmaHandle);
+    set_spi_msp_functions(&hal_spi_msp_init_dma, spiHandle, &hal_spi_msp_deinit_dma, spiHandle);
     GPIO_InitTypeDef chipSelect = {};
+    __HAL_RCC_GPIOD_CLK_ENABLE();
     chipSelect.Pin = GPIO_PIN_14;
     chipSelect.Mode = GPIO_MODE_OUTPUT_PP;
     HAL_GPIO_Init(GPIOD, &chipSelect);
-    HAL_GPIO_WritePin(GPIOD, 14, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
 }
 
 ReturnValue_t GyroL3GD20H::initialize() {
     // Configure the SPI peripheral
     spiHandle->Instance               = SPI1;
-    //uint32_t test = HAL_RCC_GetHCLKFreq();
     spiHandle->Init.BaudRatePrescaler = spi::getPrescaler(HAL_RCC_GetHCLKFreq(), 3900000);
     spiHandle->Init.Direction         = SPI_DIRECTION_2LINES;
     spi::assignSpiMode(spi::SpiModes::MODE_3, spiHandle);
@@ -63,7 +65,7 @@ ReturnValue_t GyroL3GD20H::performOperation() {
     txBuffer[1] = 0;
 
     // Start SPI transfer via DMA
-    HAL_GPIO_WritePin(GPIOD, 14, GPIO_PIN_RESET);
+    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
     if(HAL_SPI_TransmitReceive_DMA(spiHandle, txBuffer.data(), rxBuffer.data(), 2) != HAL_OK) {
         // Transfer error in transmission process
         sif::printWarning("Error transmitting SPI with DMA\n");
@@ -71,7 +73,7 @@ ReturnValue_t GyroL3GD20H::performOperation() {
 
     // Wait for the transfer to complete
     while (transferState == TransferStates::WAIT) {
-        TaskFactory::delayTask(1);
+        //TaskFactory::delayTask(1);
     }
 
     // Invalidate cache prior to access by CPU
@@ -83,7 +85,7 @@ ReturnValue_t GyroL3GD20H::performOperation() {
         uint8_t whoAmIVal = rxBuffer[1];
         if(whoAmIVal != EXPECTED_WHO_AM_I_VAL) {
             sif::printDebug("GyroL3GD20H::performOperation: "
-                    "Read WHO AM I value not equal to expected value!\n");
+                    "Read WHO AM I value %d not equal to expected value!\n", whoAmIVal);
         }
         transferState = TransferStates::IDLE;
         break;
@@ -109,7 +111,7 @@ ReturnValue_t GyroL3GD20H::performOperation() {
  */
 void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
     transferState = TransferStates::SUCCESS;
-    HAL_GPIO_WritePin(GPIOD, 14, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
 }
 
 /**
diff --git a/stm32h7/spi/CMakeLists.txt b/stm32h7/spi/CMakeLists.txt
index b1a3dcb..ea980e0 100644
--- a/stm32h7/spi/CMakeLists.txt
+++ b/stm32h7/spi/CMakeLists.txt
@@ -2,4 +2,5 @@ target_sources(${TARGET_NAME} PRIVATE
     spiCore.c
     spiDefinitions.cpp
     interrupts.c
+    mspInit.c
 )
diff --git a/stm32h7/spi/mspInit.c b/stm32h7/spi/mspInit.c
new file mode 100644
index 0000000..864f3da
--- /dev/null
+++ b/stm32h7/spi/mspInit.c
@@ -0,0 +1,192 @@
+#include "mspInit.h"
+#include "spiConf.h"
+#include "spiCore.h"
+#include "../dma_interrupts.h"
+#include "interrupts.h"
+
+#include "stm32h743xx.h"
+#include "stm32h7xx_hal_spi.h"
+#include "stm32h7xx_hal_dma.h"
+#include "stm32h7xx_hal_def.h"
+
+#include <stdio.h>
+
+void dma_rx_irq_handler(void* dma_handle);
+void dma_tx_irq_handler(void* dma_handle);
+void spi1_irq_handler(void* spi_handle);
+
+/**
+ * @brief SPI MSP Initialization
+ *        This function configures the hardware resources used in this example:
+ *           - Peripheral's clock enable
+ *           - Peripheral's GPIO Configuration
+ *           - DMA configuration for transmission request by peripheral
+ *           - NVIC configuration for DMA interrupt request enable
+ * @param hspi: SPI handle pointer
+ * @retval None
+ */
+void hal_spi_msp_init_dma(void *spi_handle) {
+    SPI_HandleTypeDef* hspi = (SPI_HandleTypeDef*) spi_handle;
+    if(hspi == NULL) {
+        return;
+    }
+    if(hspi == NULL) {
+        printf("HAL_SPI_MspInit: Invalid SPI handle!\n");
+        return;
+    }
+    assign_spi_handle(hspi);
+
+    DMA_HandleTypeDef* hdma_tx = NULL;
+    DMA_HandleTypeDef* hdma_rx = NULL;
+    get_dma_handles(&hdma_tx, &hdma_rx);
+    if(hdma_tx == NULL || hdma_rx == NULL) {
+        printf("HAL_SPI_MspInit: Invalid DMA handles. Make sure to call setDmaHandles!\n");
+        return;
+    }
+
+    GPIO_InitTypeDef GPIO_InitStruct;
+    if (hspi->Instance == SPI1)
+    {
+        /*##-1- Enable peripherals and GPIO Clocks #################################*/
+        /* Enable GPIO TX/RX clock */
+        SPIx_SCK_GPIO_CLK_ENABLE();
+        SPIx_MISO_GPIO_CLK_ENABLE();
+        SPIx_MOSI_GPIO_CLK_ENABLE();
+        /* Enable SPI1 clock */
+        SPIx_CLK_ENABLE();
+        /* Enable DMA clock */
+        DMAx_CLK_ENABLE();
+
+        /*##-2- Configure peripheral GPIO ##########################################*/
+        /* SPI SCK GPIO pin configuration  */
+        GPIO_InitStruct.Pin       = SPIx_SCK_PIN;
+        GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
+        GPIO_InitStruct.Pull      = GPIO_PULLDOWN;
+        GPIO_InitStruct.Speed     = GPIO_SPEED_FREQ_HIGH;
+        GPIO_InitStruct.Alternate = SPIx_SCK_AF;
+        HAL_GPIO_Init(SPIx_SCK_GPIO_PORT, &GPIO_InitStruct);
+
+        /* SPI MISO GPIO pin configuration  */
+        GPIO_InitStruct.Pin = SPIx_MISO_PIN;
+        GPIO_InitStruct.Alternate = SPIx_MISO_AF;
+        HAL_GPIO_Init(SPIx_MISO_GPIO_PORT, &GPIO_InitStruct);
+
+        /* SPI MOSI GPIO pin configuration  */
+        GPIO_InitStruct.Pin = SPIx_MOSI_PIN;
+        GPIO_InitStruct.Alternate = SPIx_MOSI_AF;
+        HAL_GPIO_Init(SPIx_MOSI_GPIO_PORT, &GPIO_InitStruct);
+
+        /*##-3- Configure the DMA ##################################################*/
+        /* Configure the DMA handler for Transmission process */
+        hdma_tx->Instance                 = SPIx_TX_DMA_STREAM;
+        hdma_tx->Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
+        hdma_tx->Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
+        hdma_tx->Init.MemBurst            = DMA_MBURST_INC4;
+        hdma_tx->Init.PeriphBurst         = DMA_PBURST_INC4;
+        hdma_tx->Init.Request             = SPIx_TX_DMA_REQUEST;
+        hdma_tx->Init.Direction           = DMA_MEMORY_TO_PERIPH;
+        hdma_tx->Init.PeriphInc           = DMA_PINC_DISABLE;
+        hdma_tx->Init.MemInc              = DMA_MINC_ENABLE;
+        hdma_tx->Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+        hdma_tx->Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
+        hdma_tx->Init.Mode                = DMA_NORMAL;
+        hdma_tx->Init.Priority            = DMA_PRIORITY_LOW;
+
+        HAL_DMA_Init(hdma_tx);
+
+        /* Associate the initialized DMA handle to the the SPI handle */
+        __HAL_LINKDMA(hspi, hdmatx, *hdma_tx);
+
+        /* Configure the DMA handler for Transmission process */
+        hdma_rx->Instance                 = SPIx_RX_DMA_STREAM;
+
+        hdma_rx->Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
+        hdma_rx->Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
+        hdma_rx->Init.MemBurst            = DMA_MBURST_INC4;
+        hdma_rx->Init.PeriphBurst         = DMA_PBURST_INC4;
+        hdma_rx->Init.Request             = SPIx_RX_DMA_REQUEST;
+        hdma_rx->Init.Direction           = DMA_PERIPH_TO_MEMORY;
+        hdma_rx->Init.PeriphInc           = DMA_PINC_DISABLE;
+        hdma_rx->Init.MemInc              = DMA_MINC_ENABLE;
+        hdma_rx->Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+        hdma_rx->Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
+        hdma_rx->Init.Mode                = DMA_NORMAL;
+        hdma_rx->Init.Priority            = DMA_PRIORITY_HIGH;
+
+        HAL_DMA_Init(hdma_rx);
+
+        /* Associate the initialized DMA handle to the the SPI handle */
+        __HAL_LINKDMA(hspi, hdmarx, *hdma_rx);
+
+        /*##-4- Configure the NVIC for DMA #########################################*/
+        /* NVIC configuration for DMA transfer complete interrupt (SPI1_RX) */
+        // Assign the interrupt handler
+        assign_dma_user_handler(DMA_2, 2, &dma_rx_irq_handler, hdma_rx);
+        HAL_NVIC_SetPriority(SPIx_DMA_RX_IRQn, 1, 0);
+        HAL_NVIC_EnableIRQ(SPIx_DMA_RX_IRQn);
+
+        /* NVIC configuration for DMA transfer complete interrupt (SPI1_TX) */
+        // Assign the interrupt handler
+        assign_dma_user_handler(DMA_2, 3, &dma_tx_irq_handler, hdma_tx);
+        HAL_NVIC_SetPriority(SPIx_DMA_TX_IRQn, 1, 1);
+        HAL_NVIC_EnableIRQ(SPIx_DMA_TX_IRQn);
+
+        /*##-5- Configure the NVIC for SPI #########################################*/
+        /* NVIC configuration for SPI transfer complete interrupt (SPI1) */
+        // Assign the interrupt handler
+        assign_spi_user_handler(SPI_1, &spi1_irq_handler, hspi);
+        HAL_NVIC_SetPriority(SPIx_IRQn, 1, 0);
+        HAL_NVIC_EnableIRQ(SPIx_IRQn);
+    }
+}
+
+/**
+ * @brief SPI MSP De-Initialization
+ *        This function frees the hardware resources used in this example:
+ *          - Disable the Peripheral's clock
+ *          - Revert GPIO, DMA and NVIC configuration to their default state
+ * @param hspi: SPI handle pointer
+ * @retval None
+ */
+void hal_spi_msp_deinit_dma(void *spi_handle)
+{
+    SPI_HandleTypeDef* hspi = (SPI_HandleTypeDef*) spi_handle;
+    if(hspi == NULL) {
+        return;
+    }
+    if(hspi->Instance == SPIx)
+    {
+        /*##-1- Reset peripherals ##################################################*/
+        SPIx_FORCE_RESET();
+        SPIx_RELEASE_RESET();
+
+        /*##-2- Disable peripherals and GPIO Clocks ################################*/
+        /* Deconfigure SPI SCK */
+        HAL_GPIO_DeInit(SPIx_SCK_GPIO_PORT, SPIx_SCK_PIN);
+        /* Deconfigure SPI MISO */
+        HAL_GPIO_DeInit(SPIx_MISO_GPIO_PORT, SPIx_MISO_PIN);
+        /* Deconfigure SPI MOSI */
+        HAL_GPIO_DeInit(SPIx_MOSI_GPIO_PORT, SPIx_MOSI_PIN);
+
+        DMA_HandleTypeDef* hdma_tx = NULL;
+        DMA_HandleTypeDef* hdma_rx = NULL;
+        get_dma_handles(&hdma_tx, &hdma_rx);
+        if(hdma_tx == NULL || hdma_rx == NULL) {
+            printf("HAL_SPI_MspInit: Invalid DMA handles. Make sure to call setDmaHandles!\n");
+        }
+        else {
+            /*##-3- Disable the DMA ####################################################*/
+            /* De-Initialize the DMA associated to transmission process */
+            HAL_DMA_DeInit(hdma_tx);
+            /* De-Initialize the DMA associated to reception process */
+            HAL_DMA_DeInit(hdma_rx);
+        }
+
+        /*##-4- Disable the NVIC for DMA ###########################################*/
+        HAL_NVIC_DisableIRQ(SPIx_DMA_TX_IRQn);
+        HAL_NVIC_DisableIRQ(SPIx_DMA_RX_IRQn);
+
+        /*##-5- Disable the NVIC for SPI ###########################################*/
+        HAL_NVIC_EnableIRQ(SPIx_IRQn);
+    }
+}
diff --git a/stm32h7/spi/mspInit.h b/stm32h7/spi/mspInit.h
new file mode 100644
index 0000000..9ac8562
--- /dev/null
+++ b/stm32h7/spi/mspInit.h
@@ -0,0 +1,15 @@
+#ifndef FSFW_HAL_STM32H7_SPI_MSPINIT_H_
+#define FSFW_HAL_STM32H7_SPI_MSPINIT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void hal_spi_msp_init_dma(void *hspi);
+void hal_spi_msp_deinit_dma(void *hspi);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* FSFW_HAL_STM32H7_SPI_MSPINIT_H_ */
diff --git a/stm32h7/spi/spiCore.c b/stm32h7/spi/spiCore.c
index 1d81b5e..2516dbf 100644
--- a/stm32h7/spi/spiCore.c
+++ b/stm32h7/spi/spiCore.c
@@ -1,27 +1,34 @@
 #include "spiCore.h"
+#include <stdio.h>
 
 SPI_HandleTypeDef* spiHandle = NULL;
 DMA_HandleTypeDef* hdma_tx = NULL;
 DMA_HandleTypeDef* hdma_rx = NULL;
 
-void setDmaHandles(DMA_HandleTypeDef* txHandle, DMA_HandleTypeDef* rxHandle) {
+msp_func_t msp_init_func = NULL;
+void* msp_init_args = NULL;
+
+msp_func_t msp_deinit_func = NULL;
+void* msp_deinit_args = NULL;
+
+void set_dma_handles(DMA_HandleTypeDef* txHandle, DMA_HandleTypeDef* rxHandle) {
     hdma_tx = txHandle;
     hdma_rx = rxHandle;
 }
 
-void getDmaHandles(DMA_HandleTypeDef** txHandle, DMA_HandleTypeDef** rxHandle) {
+void get_dma_handles(DMA_HandleTypeDef** txHandle, DMA_HandleTypeDef** rxHandle) {
     *txHandle = hdma_tx;
     *rxHandle = hdma_rx;
 }
 
-void assignSpiHandle(SPI_HandleTypeDef *spiHandle_) {
+void assign_spi_handle(SPI_HandleTypeDef *spiHandle_) {
     if(spiHandle_ == NULL) {
         return;
     }
     spiHandle = spiHandle_;
 }
 
-SPI_HandleTypeDef* getSpiHandle() {
+SPI_HandleTypeDef* get_spi_handle() {
     return spiHandle;
 }
 
@@ -52,3 +59,61 @@ void spi1_irq_handler(void* spi_handle)
 {
     HAL_SPI_IRQHandler((SPI_HandleTypeDef *) spi_handle);
 }
+
+void set_spi_msp_functions(msp_func_t init_func, void* init_args, msp_func_t deinit_func,
+        void* deinit_args) {
+    msp_init_func = init_func;
+    msp_init_args = init_args;
+    msp_deinit_func = deinit_func;
+    msp_deinit_args = deinit_args;
+}
+
+void get_msp_init_function(msp_func_t* init_func, void **args) {
+    if(init_func != NULL && args != NULL) {
+        *init_func = msp_init_func;
+        *args = msp_init_args;
+    }
+}
+
+void get_msp_deinit_function(msp_func_t* deinit_func, void **args) {
+    if(deinit_func != NULL && args != NULL) {
+        *deinit_func = msp_deinit_func;
+        *args = msp_deinit_args;
+    }
+}
+
+/**
+ * @brief SPI MSP Initialization
+ *        This function configures the hardware resources used in this example:
+ *           - Peripheral's clock enable
+ *           - Peripheral's GPIO Configuration
+ *           - DMA configuration for transmission request by peripheral
+ *           - NVIC configuration for DMA interrupt request enable
+ * @param hspi: SPI handle pointer
+ * @retval None
+ */
+void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) {
+    if(msp_init_func != NULL) {
+        msp_init_func(msp_init_args);
+    }
+    else {
+        printf("HAL_SPI_MspInit: Please call set_msp_functions to assign SPI MSP functions\n");
+    }
+}
+
+/**
+ * @brief SPI MSP De-Initialization
+ *        This function frees the hardware resources used in this example:
+ *          - Disable the Peripheral's clock
+ *          - Revert GPIO, DMA and NVIC configuration to their default state
+ * @param hspi: SPI handle pointer
+ * @retval None
+ */
+void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi) {
+    if(msp_deinit_func != NULL) {
+        msp_deinit_func(msp_deinit_args);
+    }
+    else {
+        printf("HAL_SPI_MspDeInit: Please call set_msp_functions to assign SPI MSP functions\n");
+    }
+}
diff --git a/stm32h7/spi/spiCore.h b/stm32h7/spi/spiCore.h
index 9af0de0..75d4eca 100644
--- a/stm32h7/spi/spiCore.h
+++ b/stm32h7/spi/spiCore.h
@@ -8,26 +8,40 @@
 extern "C" {
 #endif
 
+typedef void (*msp_func_t) (void* args);
+
+/**
+ * Assign MSP init functions. Important for SPI configuration
+ * @param init_func
+ * @param init_args
+ * @param deinit_func
+ * @param deinit_args
+ */
+void set_spi_msp_functions(msp_func_t init_func, void* init_args, msp_func_t deinit_func,
+        void* deinit_args);
+
 /**
  * Assign DMA handles. Required to use DMA for SPI transfers.
  * @param txHandle
  * @param rxHandle
  */
-void setDmaHandles(DMA_HandleTypeDef* txHandle, DMA_HandleTypeDef* rxHandle);
+void set_dma_handles(DMA_HandleTypeDef* txHandle, DMA_HandleTypeDef* rxHandle);
 
-void getDmaHandles(DMA_HandleTypeDef** txHandle, DMA_HandleTypeDef** rxHandle);
+void get_dma_handles(DMA_HandleTypeDef** txHandle, DMA_HandleTypeDef** rxHandle);
+void get_msp_init_function(msp_func_t* init_func, void **args);
+void get_msp_deinit_function(msp_func_t* deinit_func, void **args);
 
 /**
  * Assign SPI handle. Needs to be done before using the SPI
  * @param spiHandle
  */
-void assignSpiHandle(SPI_HandleTypeDef *spiHandle);
+void assign_spi_handle(SPI_HandleTypeDef *spiHandle);
 
 /**
  * Get the assigned SPI handle.
  * @return
  */
-SPI_HandleTypeDef* getSpiHandle();
+SPI_HandleTypeDef* get_spi_handle();
 
 #ifdef __cplusplus
 }