diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed973ac7367ae16918f03fcb9b17d1263fe6c610..bab43939a686d7586dd75b0a22557adbb5dde95f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -285,7 +285,7 @@ add_executable(test-usart-f7 src/tests/drivers/usart/test-usart.cpp)
 sbs_target(test-usart-f7 stm32f767zi_nucleo)
 
 add_executable(test-dma-mem-to-mem src/tests/drivers/test-dma-mem-to-mem.cpp)
-sbs_target(test-dma-mem-to-mem stm32f769ni_discovery)
+sbs_target(test-dma-mem-to-mem stm32f407vg_stm32f4discovery)
 
 add_executable(test-i2c-driver-f4 src/tests/drivers/i2c/test-i2c-driver.cpp)
 sbs_target(test-i2c-driver-f4 stm32f429zi_stm32f4discovery)
diff --git a/src/shared/drivers/dma/DMA.cpp b/src/shared/drivers/dma/DMA.cpp
index 3848389799c375926a0ec004c201b0b8fab0ce41..653401f22d0e75ff66050efcd805ec9c82809318 100644
--- a/src/shared/drivers/dma/DMA.cpp
+++ b/src/shared/drivers/dma/DMA.cpp
@@ -29,9 +29,6 @@
 
 using namespace miosix;
 
-namespace Boardcore
-{
-
 void __attribute__((naked)) DMA1_Stream0_IRQHandler()
 {
     saveContext();
@@ -41,20 +38,21 @@ void __attribute__((naked)) DMA1_Stream0_IRQHandler()
 
 void __attribute__((used)) DMA1_Stream0_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str0);
-}
-
-void __attribute__((naked)) DMA1_Stream1_IRQHandler()
-{
-    saveContext();
-    asm volatile("bl _Z20DMA1_Stream1_IRQImplv");
-    restoreContext();
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA1_Str0);
 }
 
-void __attribute__((used)) DMA1_Stream1_IRQImpl()
-{
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str1);
-}
+// Commented because already used elsewhere by miosix
+// void __attribute__((naked)) DMA1_Stream1_IRQHandler()
+// {
+//     saveContext();
+//     asm volatile("bl _Z20DMA1_Stream1_IRQImplv");
+//     restoreContext();
+// }
+// void __attribute__((used)) DMA1_Stream1_IRQImpl()
+// {
+//     Boardcore::DMADriver::instance().IRQhandleInterrupt(Boardcore::DMAStreamId::DMA1_Str1);
+// }
 
 void __attribute__((naked)) DMA1_Stream2_IRQHandler()
 {
@@ -65,20 +63,21 @@ void __attribute__((naked)) DMA1_Stream2_IRQHandler()
 
 void __attribute__((used)) DMA1_Stream2_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str2);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA1_Str2);
 }
 
-void __attribute__((naked)) DMA1_Stream3_IRQHandler()
-{
-    saveContext();
-    asm volatile("bl _Z20DMA1_Stream3_IRQImplv");
-    restoreContext();
-}
-
-void __attribute__((used)) DMA1_Stream3_IRQImpl()
-{
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str3);
-}
+// Commented because already used elsewhere by miosix
+// void __attribute__((naked)) DMA1_Stream3_IRQHandler()
+// {
+//     saveContext();
+//     asm volatile("bl _Z20DMA1_Stream3_IRQImplv");
+//     restoreContext();
+// }
+// void __attribute__((used)) DMA1_Stream3_IRQImpl()
+// {
+//     Boardcore::DMADriver::instance().IRQhandleInterrupt(Boardcore::DMAStreamId::DMA1_Str3);
+// }
 
 void __attribute__((naked)) DMA1_Stream4_IRQHandler()
 {
@@ -89,7 +88,8 @@ void __attribute__((naked)) DMA1_Stream4_IRQHandler()
 
 void __attribute__((used)) DMA1_Stream4_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str4);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA1_Str4);
 }
 
 void __attribute__((naked)) DMA1_Stream5_IRQHandler()
@@ -101,7 +101,8 @@ void __attribute__((naked)) DMA1_Stream5_IRQHandler()
 
 void __attribute__((used)) DMA1_Stream5_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str5);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA1_Str5);
 }
 
 void __attribute__((naked)) DMA1_Stream6_IRQHandler()
@@ -113,7 +114,8 @@ void __attribute__((naked)) DMA1_Stream6_IRQHandler()
 
 void __attribute__((used)) DMA1_Stream6_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str6);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA1_Str6);
 }
 
 void __attribute__((naked)) DMA1_Stream7_IRQHandler()
@@ -125,7 +127,8 @@ void __attribute__((naked)) DMA1_Stream7_IRQHandler()
 
 void __attribute__((used)) DMA1_Stream7_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA1_Str7);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA1_Str7);
 }
 
 void __attribute__((naked)) DMA2_Stream0_IRQHandler()
@@ -137,7 +140,8 @@ void __attribute__((naked)) DMA2_Stream0_IRQHandler()
 
 void __attribute__((used)) DMA2_Stream0_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA2_Str0);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA2_Str0);
 }
 
 void __attribute__((naked)) DMA2_Stream1_IRQHandler()
@@ -149,7 +153,8 @@ void __attribute__((naked)) DMA2_Stream1_IRQHandler()
 
 void __attribute__((used)) DMA2_Stream1_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA2_Str1);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA2_Str1);
 }
 
 void __attribute__((naked)) DMA2_Stream2_IRQHandler()
@@ -161,7 +166,8 @@ void __attribute__((naked)) DMA2_Stream2_IRQHandler()
 
 void __attribute__((used)) DMA2_Stream2_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA2_Str2);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA2_Str2);
 }
 
 // void __attribute__((naked)) DMA2_Stream3_IRQHandler() {
@@ -183,7 +189,8 @@ void __attribute__((naked)) DMA2_Stream4_IRQHandler()
 
 void __attribute__((used)) DMA2_Stream4_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA2_Str4);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA2_Str4);
 }
 
 // void __attribute__((naked)) DMA2_Stream5_IRQHandler() {
@@ -205,7 +212,8 @@ void __attribute__((naked)) DMA2_Stream6_IRQHandler()
 
 void __attribute__((used)) DMA2_Stream6_IRQImpl()
 {
-    DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA2_Str6);
+    Boardcore::DMADriver::instance().IRQhandleInterrupt(
+        Boardcore::DMAStreamId::DMA2_Str6);
 }
 
 // void __attribute__((naked)) DMA2_Stream7_IRQHandler() {
@@ -218,6 +226,9 @@ void __attribute__((used)) DMA2_Stream6_IRQImpl()
 //     DMADriver::instance().IRQhandleInterrupt(DMAStreamId::DMA2_Str7);
 // }
 
+namespace Boardcore
+{
+
 void DMADriver::IRQhandleInterrupt(DMAStreamId id)
 {
     auto stream = streams[id];
@@ -320,9 +331,9 @@ DMADriver::DMADriver()
     ClockUtils::enablePeripheralClock(DMA1);
     ClockUtils::enablePeripheralClock(DMA2);
 
-    // Reset interrupts flags
+    // Reset interrupts flags by setting the clear bits to 1
     // TODO: Change this magic number
-    DMA1->HIFCR = 0x0f7d0f7d;
+    DMA1->HIFCR = 0x0f7d0f7d;  // = 0b 0000 1111 0111 1101 0000 1111 0111 1101
     DMA1->LIFCR = 0x0f7d0f7d;
     DMA2->HIFCR = 0x0f7d0f7d;
     DMA2->LIFCR = 0x0f7d0f7d;
@@ -443,6 +454,11 @@ void DMAStream::enable()
     fifoErrorFlag        = false;
     directModeErrorFlag  = false;
 
+    // Before setting EN bit to '1' to start a new transfer, the event
+    //  flags corresponding to the stream in DMA_LISR or DMA_HISR
+    //  register must be cleared.
+    clearAllFlags();
+
     // Enable the peripheral
     registers->CR |= DMA_SxCR_EN;
 }
@@ -532,15 +548,17 @@ DMAStream::DMAStream(DMAStreamId id) : id(id)
     if (id < DMAStreamId::DMA2_Str0)
     {
         registers = reinterpret_cast<DMA_Stream_TypeDef*>(
-            DMA1_BASE + 0x10 + 0x18 * static_cast<int>(id));
+            DMA1_BASE + 0x10 + 0x18 * static_cast<uint8_t>(id));
 
         if (id < DMAStreamId::DMA1_Str4)
         {
+            // Streams from 0 to 3 use low registers (LIFCR and LISR)
             IFCR = &DMA1->LIFCR;
             ISR  = &DMA1->LISR;
         }
         else
         {
+            // Streams from 4 to 7 use high registers (HIFCR and HISR)
             IFCR = &DMA1->HIFCR;
             ISR  = &DMA1->HISR;
         }
@@ -548,15 +566,17 @@ DMAStream::DMAStream(DMAStreamId id) : id(id)
     else
     {
         registers = reinterpret_cast<DMA_Stream_TypeDef*>(
-            DMA2_BASE + 0x10 + 0x18 * (static_cast<int>(id) - 8));
+            DMA2_BASE + 0x10 + 0x18 * (static_cast<uint8_t>(id) - 8));
 
         if (id < DMAStreamId::DMA2_Str4)
         {
+            // Streams from 0 to 3 use low registers (LIFCR and LISR)
             IFCR = &DMA2->LIFCR;
             ISR  = &DMA2->LISR;
         }
         else
         {
+            // Streams from 4 to 7 use high registers (HIFCR and HISR)
             IFCR = &DMA2->HIFCR;
             ISR  = &DMA2->HISR;
         }
@@ -564,12 +584,11 @@ DMAStream::DMAStream(DMAStreamId id) : id(id)
 
     // Compute the index for the interrupt flags clear register
     // Refer to reference manual for the register bits structure
-    int offset = static_cast<int>(id) % 4;
+    int offset = static_cast<uint8_t>(id) % 4;
     IFindex    = (offset % 2) * 6 + (offset / 2) * 16;
 
     // Select the interrupt
-    irqNumber = static_cast<IRQn_Type>(
-        static_cast<int>(IRQn_Type::DMA1_Stream0_IRQn) + static_cast<int>(id));
+    irqNumber = irqNumberMapping[static_cast<uint8_t>(id)];
 }
 
 }  // namespace Boardcore
diff --git a/src/shared/drivers/dma/DMA.h b/src/shared/drivers/dma/DMA.h
index b0e31d3df58c2936e624c61cb6f144efa6ea92ab..a0262f0e7713a1f7f01fe8f8c50d4ecbfcd18913 100644
--- a/src/shared/drivers/dma/DMA.h
+++ b/src/shared/drivers/dma/DMA.h
@@ -33,24 +33,45 @@
 namespace Boardcore
 {
 
-enum class DMAStreamId
+// TODO: remove and find a better solution for stm32f407
+#ifndef DMA_SxCR_MSIZE_Pos
+#define DMA_SxCR_MSIZE_Pos (13U)
+#endif
+// TODO: remove and find a better solution for stm32f407
+#ifndef DMA_SxCR_PSIZE_Pos
+#define DMA_SxCR_PSIZE_Pos (11U)
+#endif
+
+enum class DMAStreamId : uint8_t
 {
     DMA1_Str0 = 0,
-    DMA1_Str1,
-    DMA1_Str2,
-    DMA1_Str3,
-    DMA1_Str4,
-    DMA1_Str5,
-    DMA1_Str6,
-    DMA1_Str7,
-    DMA2_Str0,
-    DMA2_Str1,
-    DMA2_Str2,
-    DMA2_Str3,
-    DMA2_Str4,
-    DMA2_Str5,
-    DMA2_Str6,
-    DMA2_Str7,
+    DMA1_Str1 = 1,
+    DMA1_Str2 = 2,
+    DMA1_Str3 = 3,
+    DMA1_Str4 = 4,
+    DMA1_Str5 = 5,
+    DMA1_Str6 = 6,
+    DMA1_Str7 = 7,
+    DMA2_Str0 = 8,
+    DMA2_Str1 = 9,
+    DMA2_Str2 = 10,
+    DMA2_Str3 = 11,
+    DMA2_Str4 = 12,
+    DMA2_Str5 = 13,
+    DMA2_Str6 = 14,
+    DMA2_Str7 = 15,
+};
+
+/**
+ * @brief Mapping between `DMAStreamId` and the corresponding irq number.
+ * This is needed because irq number values are not contiguous and they are
+ * architecture dependent.
+ */
+const IRQn_Type irqNumberMapping[] = {
+    DMA1_Stream0_IRQn, DMA1_Stream1_IRQn, DMA1_Stream2_IRQn, DMA1_Stream3_IRQn,
+    DMA1_Stream4_IRQn, DMA1_Stream5_IRQn, DMA1_Stream6_IRQn, DMA1_Stream7_IRQn,
+    DMA2_Stream0_IRQn, DMA2_Stream1_IRQn, DMA2_Stream2_IRQn, DMA2_Stream3_IRQn,
+    DMA2_Stream4_IRQn, DMA2_Stream5_IRQn, DMA2_Stream6_IRQn, DMA2_Stream7_IRQn,
 };
 
 struct DMATransaction
@@ -144,8 +165,15 @@ class DMAStream
     friend DMADriver;
 
 public:
+    /**
+     * @brief Setup the stream with the given configuration.
+     */
     void setup(DMATransaction transaction);
 
+    /**
+     * @brief Activate the stream. As soon as the stream is enabled, it
+     * can serve any DMA request from the peripheral connected to the stream.
+     */
     void enable();
 
     void disable();
@@ -241,6 +269,10 @@ public:
         *IFCR |= DMA_LIFCR_CDMEIF0 << IFindex;
     }
 
+    /**
+     * @brief Clear all the flags for the selected stream in the DMA ISR
+     * register (LISR or HISR depending on the selected stream id).
+     */
     inline void clearAllFlags()
     {
         *IFCR |= (DMA_LIFCR_CHTIF0 | DMA_LIFCR_CTCIF0 | DMA_LIFCR_CTEIF0 |
@@ -252,6 +284,11 @@ private:
     DMAStream(DMAStreamId id);
 
     DMATransaction currentSetup;
+
+    /**
+     * @brief Used to determine if the user thread is
+     * waiting to be awaked by an interrupt.
+     */
     miosix::Thread* waitingThread = nullptr;
 
     // These flags are set by the interrupt routine and tells the user
@@ -308,25 +345,22 @@ private:
                 {
                     do
                     {
-                        // TODO: Wait Miosix 2.7 or do in another way?
-                        // if (miosix::Thread::IRQenableIrqAndTimedWait(
-                        //         dLock, timeout_ns + miosix::getTime()) ==
-                        //     miosix::TimedWaitResult::Timeout)
-                        // {
-                        //     result = false;
-
-                        //     // If the timeout expired we clear the thread
-                        //     // pointer so that the interrupt, if it will
-                        //     occur,
-                        //     // will not wake up the thread (and we can exit
-                        //     the
-                        //     // while loop)
-                        //     waitingThread = nullptr;
-                        // }
-                        // else
-                        // {
-                        //     result = true;
-                        // }
+                        if (miosix::Thread::IRQenableIrqAndTimedWait(
+                                dLock, timeout_ns + miosix::getTime()) ==
+                            miosix::TimedWaitResult::Timeout)
+                        {
+                            result = false;
+
+                            // If the timeout expired we clear the thread
+                            // pointer so that the interrupt, if it will occur,
+                            // will not wake up the thread (and we can exit the
+                            // while loop)
+                            waitingThread = nullptr;
+                        }
+                        else
+                        {
+                            result = true;
+                        }
                     } while (waitingThread);
                 }
                 else
@@ -348,12 +382,20 @@ private:
             // Pool the flag if the user did not enable the interrupt
             if (timeout_ns >= 0)
             {
+                // TODO: this doesn't work, because the methods passed as
+                // getEventStatus do not update the flags (reading them
+                // by calling readFlags()).
+                // This means that it always polls a value that will
+                // never become true, then exit when the timeout is
+                // reached.
                 result = timedPollingFlag(getEventStatus, timeout_ns);
             }
             else
             {
                 while (!getEventStatus())
-                    ;
+                {
+                    readFlags();
+                }
                 result = true;
             }
 
diff --git a/src/shared/utils/TimedPollingFlag.h b/src/shared/utils/TimedPollingFlag.h
index de0574e5bdf396b876efc7d00b7a7d72ff2ade9a..4e2dd42d04bff8b98e19eba798b7339266151ffd 100644
--- a/src/shared/utils/TimedPollingFlag.h
+++ b/src/shared/utils/TimedPollingFlag.h
@@ -37,10 +37,9 @@ namespace Boardcore
 inline bool timedPollingFlag(std::function<bool()> readFlag,
                              uint64_t timeout_ns)
 {
-    // TODO: When Miosix 2.7 will be supported, change this with getTime() in ns
-    uint64_t start = miosix::getTick();
+    uint64_t start = miosix::getTime();
 
-    while (miosix::getTick() - start < timeout_ns * 1e6)
+    while (miosix::getTime() - start < timeout_ns)
     {
         if (readFlag())
         {
@@ -51,4 +50,4 @@ inline bool timedPollingFlag(std::function<bool()> readFlag,
     return false;
 }
 
-}  // namespace Boardcore
\ No newline at end of file
+}  // namespace Boardcore
diff --git a/src/tests/drivers/test-dma-mem-to-mem.cpp b/src/tests/drivers/test-dma-mem-to-mem.cpp
index 97f43983c3a013a1b47eed16cf1df4af9412960e..f11549bcd9695c0d667caf51d9cff71390631f96 100644
--- a/src/tests/drivers/test-dma-mem-to-mem.cpp
+++ b/src/tests/drivers/test-dma-mem-to-mem.cpp
@@ -24,6 +24,8 @@
 #include <miosix.h>
 #include <util/util.h>
 
+#include <chrono>
+
 using namespace miosix;
 using namespace Boardcore;
 
@@ -40,6 +42,7 @@ int main()
     uint8_t buffer1[8] = {1, 2, 3, 4, 5, 6, 7, 8};
     uint8_t buffer2[8] = {0};
 
+    printf("Before:\n");
     printf("Buffer 1:\n");
     printBuffer(buffer1, sizeof(buffer1));
     printf("Buffer 2:\n");
@@ -54,18 +57,27 @@ int main()
         .numberOfDataItems = sizeof(buffer1),
         .srcIncrement      = true,
         .dstIncrement      = true,
+        .enableTransferCompleteInterrupt = true,
     };
     stream.setup(trn);
     stream.enable();
-    // stream.waitForTransferComplete();
-    delayMs(10);
 
-    delayMs(100);
+    auto begin = std::chrono::steady_clock::now();
+    stream.waitForTransferComplete();
+    auto end = std::chrono::steady_clock::now();
 
+    printf("After:\n");
     printf("Buffer 1:\n");
     printBuffer(buffer1, sizeof(buffer1));
     printf("Buffer 2:\n");
     printBuffer(buffer2, sizeof(buffer2));
+
+    const auto elapsedTime =
+        std::chrono::duration_cast<std::chrono::microseconds>(end - begin)
+            .count();
+    printf("Elapsed time: %lld [us]\n", elapsedTime);
+
+    return 0;
 }
 
 void printBuffer(uint8_t *buffer, size_t size)
@@ -76,4 +88,4 @@ void printBuffer(uint8_t *buffer, size_t size)
     }
 
     printf("%x\n", buffer[size - 1]);
-}
\ No newline at end of file
+}