diff --git a/examples/Beginner/Waveform_Generator/Waveform_Generator.ino b/examples/Beginner/Waveform_Generator/Waveform_Generator.ino
index 5ba5494..a5dbb21 100644
--- a/examples/Beginner/Waveform_Generator/Waveform_Generator.ino
+++ b/examples/Beginner/Waveform_Generator/Waveform_Generator.ino
@@ -1,22 +1,43 @@
 // This example generates different waveforms based on user input on A12/DAC1.
 
 #include <Arduino_AdvancedAnalog.h>
+#include <mbed_stats.h>
 
-#define N_SAMPLES           (256)
-#define DEFAULT_FREQUENCY   (16000)
+#define N_SAMPLES           (64)
+#define DEFAULT_FREQUENCY   (32000)
 
 AdvancedDAC dac1(A12);
 uint8_t SAMPLES_BUFFER[N_SAMPLES];
-size_t dac_frequency = DEFAULT_FREQUENCY;
 
-void generate_waveform(int cmd) 
-{   
+uint32_t get_current_heap() {
+    mbed_stats_heap_t heap_stats;
+    mbed_stats_heap_get(&heap_stats);
+    return heap_stats.current_size;
+}
+
+void print_menu() {
+    Serial.println();
+    Serial.println("Enter a command:");
+    Serial.println("t: Triangle wave");
+    Serial.println("q: Square wave");
+    Serial.println("s: Sine wave");
+    Serial.println("r: Sawtooth wave");
+    Serial.println("k: stop DAC");
+    Serial.println("+: Increase frequency");
+    Serial.println("-: Decrease frequency");
+}
+
+void generate_waveform(int cmd) {
+    static bool dac_started = false;
+    static size_t dac_frequency = DEFAULT_FREQUENCY;
+    static size_t starting_heap = get_current_heap();
+
     switch (cmd) {
         case 't':
             // Triangle wave
             Serial.print("Waveform: Triangle ");
-            for (int i=0; i<N_SAMPLES; i++){
-                SAMPLES_BUFFER[i] = abs((i % 255) - 127);
+            for (int i=0, j=N_SAMPLES-1; i<N_SAMPLES; i++, j--){
+                SAMPLES_BUFFER[i] = abs((i - j) * 256 / N_SAMPLES);
             }
             break;
 
@@ -24,7 +45,7 @@ void generate_waveform(int cmd)
             // Square wave
             Serial.print("Waveform: Square ");
             for (int i=0; i<N_SAMPLES; i++){
-                SAMPLES_BUFFER[i] = (i % 255) < 127 ? 127 : 0;
+                SAMPLES_BUFFER[i] = i < (N_SAMPLES / 2) ? 0 : 255;
             }
             break;
 
@@ -38,36 +59,59 @@ void generate_waveform(int cmd)
 
         case 'r':
             // Sawtooth
-            Serial.print("Waveform: Sawtooth");
+            Serial.print("Waveform: Sawtooth ");
             for (int i=0; i<N_SAMPLES; i++){
-                SAMPLES_BUFFER[i] = i;
+                SAMPLES_BUFFER[i] = i * (256 / N_SAMPLES);
             }
             break;
 
         case '+':
         case '-':
             Serial.print("Current frequency: ");
-            
-            if (cmd == '+' && dac_frequency < 64000) {
-              dac_frequency *= 2;
+
+            if (cmd == '+' && dac_frequency < 128000) {
+                dac_frequency *= 2;
             } else if (cmd == '-' && dac_frequency > 1000) {
-              dac_frequency /= 2;
+                dac_frequency /= 2;
             } else {
-              break;
+                break;
             }
 
             // Change frequency.
             dac1.frequency(dac_frequency * N_SAMPLES);
             break;
-            
+
+        case 'k':
+            dac1.stop();
+            dac_started = false;
+            break;
+
         default:
             Serial.print("Unknown command ");
             Serial.println((char) cmd);
             return;
     }
-    
-    Serial.print(dac_frequency/1000);
-    Serial.println("KHz");
+
+    if (cmd == 'k') {
+        Serial.println("DAC stopped!");
+        print_menu();
+    } else {
+        Serial.print(dac_frequency/1000);
+        Serial.println("KHz");
+
+        if (dac_started == false) {
+            // Initialize and start the DAC.
+            if (!dac1.begin(AN_RESOLUTION_8, dac_frequency * N_SAMPLES, N_SAMPLES, 32)) {
+                Serial.println("Failed to start DAC1 !");
+                while (1);
+            }
+            dac_started = true;
+        }
+    }
+
+    Serial.print("Used memory: ");
+    Serial.print(get_current_heap() - starting_heap);
+    Serial.println(" bytes");
 }
 
 void setup() {
@@ -77,32 +121,21 @@ void setup() {
 
     }
 
-    
-    Serial.println("Enter a command:");
-    Serial.println("t: Triangle wave");
-    Serial.println("q: Square wave");
-    Serial.println("s: Sine wave");
-    Serial.println("r: Sawtooth wave");
-    Serial.println("+: Increase frequency");
-    Serial.println("-: Decrease frequency");
-    
+    // Print list of commands.
+    print_menu();
+
+    // Start generating a sine wave.
     generate_waveform('s');
-    
-    // DAC initialization
-    if (!dac1.begin(AN_RESOLUTION_8, DEFAULT_FREQUENCY * N_SAMPLES, N_SAMPLES, 32)) {
-        Serial.println("Failed to start DAC1 !");
-        while (1);
-    }
 }
 
 void loop() {
     if (Serial.available() > 0) {
         int cmd = Serial.read();
         if (cmd != '\n') {
-          generate_waveform(cmd);
+            generate_waveform(cmd);
         }
     } 
-    
+
     if (dac1.available()) {
         // Get a free buffer for writing.
         SampleBuffer buf = dac1.dequeue();
diff --git a/src/AdvancedDAC.cpp b/src/AdvancedDAC.cpp
index 644f500..54684dd 100644
--- a/src/AdvancedDAC.cpp
+++ b/src/AdvancedDAC.cpp
@@ -29,6 +29,7 @@ struct dac_descr_t {
     TIM_HandleTypeDef tim;
     uint32_t tim_trig;
     uint32_t resolution;
+    uint32_t dmaudr_flag;
     DMABufferPool<Sample> *pool;
     DMABuffer<Sample> *dmabuf[2];
 };
@@ -37,10 +38,10 @@ struct dac_descr_t {
 static DAC_HandleTypeDef dac = {0};
 
 static dac_descr_t dac_descr_all[] = {
-    {&dac, DAC_CHANNEL_1, {DMA1_Stream4, {DMA_REQUEST_DAC1_CH1}}, DMA1_Stream4_IRQn,
-        {TIM4}, DAC_TRIGGER_T4_TRGO, DAC_ALIGN_12B_R, nullptr, {nullptr, nullptr}},
-    {&dac, DAC_CHANNEL_2, {DMA1_Stream5, {DMA_REQUEST_DAC1_CH2}}, DMA1_Stream5_IRQn,
-        {TIM5}, DAC_TRIGGER_T5_TRGO, DAC_ALIGN_12B_R, nullptr, {nullptr, nullptr}},
+    {&dac, DAC_CHANNEL_1, {DMA1_Stream4, {DMA_REQUEST_DAC1_CH1}}, DMA1_Stream4_IRQn, {TIM4},
+        DAC_TRIGGER_T4_TRGO, DAC_ALIGN_12B_R, DAC_FLAG_DMAUDR1, nullptr, {nullptr, nullptr}},
+    {&dac, DAC_CHANNEL_2, {DMA1_Stream5, {DMA_REQUEST_DAC1_CH2}}, DMA1_Stream5_IRQn, {TIM5},
+        DAC_TRIGGER_T5_TRGO, DAC_ALIGN_12B_R, DAC_FLAG_DMAUDR2, nullptr, {nullptr, nullptr}},
 };
 
 static uint32_t DAC_RES_LUT[] = {
@@ -73,10 +74,12 @@ static dac_descr_t *dac_descr_get(uint32_t channel) {
 }
 
 static void dac_descr_deinit(dac_descr_t *descr, bool dealloc_pool) {
-    if (descr) {
+    if (descr != nullptr) {
         HAL_TIM_Base_Stop(&descr->tim);
         HAL_DAC_Stop_DMA(descr->dac, descr->channel);
 
+        __HAL_DAC_CLEAR_FLAG(descr->dac, descr->dmaudr_flag);
+
         for (size_t i=0; i<AN_ARRAY_SIZE(descr->dmabuf); i++) {
             if (descr->dmabuf[i]) {
                 descr->dmabuf[i]->release();
@@ -89,13 +92,17 @@ static void dac_descr_deinit(dac_descr_t *descr, bool dealloc_pool) {
                 delete descr->pool;
             }
             descr->pool = nullptr;
+        } else {
+            descr->pool->flush();
         }
-
     }
 }
 
 bool AdvancedDAC::available() {
     if (descr != nullptr) {
+        if (__HAL_DAC_GET_FLAG(descr->dac, descr->dmaudr_flag)) {
+            dac_descr_deinit(descr, false);
+        }
         return descr->pool->writable();
     }
     return false;
@@ -113,11 +120,17 @@ DMABuffer<Sample> &AdvancedDAC::dequeue() {
 }
 
 void AdvancedDAC::write(DMABuffer<Sample> &dmabuf) {
+    static uint32_t buf_count = 0;
+
+    if (descr == nullptr) {
+        return;
+    }
+
     // Make sure any cached data is flushed.
     dmabuf.flush();
     descr->pool->enqueue(&dmabuf);
 
-    if (descr->dmabuf[0] == nullptr && descr->pool->readable() > 2) {
+    if (descr->dmabuf[0] == nullptr && (++buf_count % 3) == 0) {
         descr->dmabuf[0] = descr->pool->dequeue();
         descr->dmabuf[1] = descr->pool->dequeue();
 
@@ -126,7 +139,9 @@ void AdvancedDAC::write(DMABuffer<Sample> &dmabuf) {
         (uint32_t *) descr->dmabuf[0]->data(), descr->dmabuf[0]->size(), descr->resolution);
 
         // Re/enable DMA double buffer mode.
+        HAL_NVIC_DisableIRQ(descr->dma_irqn);
         hal_dma_enable_dbm(&descr->dma, descr->dmabuf[0]->data(), descr->dmabuf[1]->data());
+        HAL_NVIC_EnableIRQ(descr->dma_irqn);
 
         // Start trigger timer.
         HAL_TIM_Base_Start(&descr->tim);
@@ -135,7 +150,7 @@ void AdvancedDAC::write(DMABuffer<Sample> &dmabuf) {
 
 int AdvancedDAC::begin(uint32_t resolution, uint32_t frequency, size_t n_samples, size_t n_buffers) {
     // Sanity checks.
-    if (resolution >= AN_ARRAY_SIZE(DAC_RES_LUT) || (descr && descr->pool)) {
+    if (resolution >= AN_ARRAY_SIZE(DAC_RES_LUT) || descr != nullptr) {
         return 0;
     }
 
@@ -147,13 +162,14 @@ int AdvancedDAC::begin(uint32_t resolution, uint32_t frequency, size_t n_samples
 
     uint32_t function = pinmap_function(dac_pins[0], PinMap_DAC);
     descr = dac_descr_get(DAC_CHAN_LUT[STM_PIN_CHANNEL(function) - 1]);
-    if (descr == nullptr || descr->pool) {
+    if (descr == nullptr) {
         return 0;
     }
 
     // Allocate DMA buffer pool.
     descr->pool = new DMABufferPool<Sample>(n_samples, n_channels, n_buffers);
     if (descr->pool == nullptr) {
+        descr = nullptr;
         return 0;
     }
     descr->resolution = DAC_RES_LUT[resolution];
@@ -178,13 +194,16 @@ int AdvancedDAC::begin(uint32_t resolution, uint32_t frequency, size_t n_samples
 
 int AdvancedDAC::stop()
 {
-    dac_descr_deinit(descr, true);
+    if (descr != nullptr) {
+        dac_descr_deinit(descr, true);
+        descr = nullptr;
+    }
     return 1;
 }
 
 int AdvancedDAC::frequency(uint32_t const frequency)
 {
-    if (descr && descr->pool) {
+    if (descr != nullptr) {
         // Reconfigure the trigger timer.
         dac_descr_deinit(descr, false);
         hal_tim_config(&descr->tim, frequency);
@@ -200,9 +219,10 @@ extern "C" {
 
 void DAC_DMAConvCplt(DMA_HandleTypeDef *dma, uint32_t channel) {
     dac_descr_t *descr = dac_descr_get(channel);
+
     // Release the DMA buffer that was just done, allocate a new one,
     // and update the next DMA memory address target.
-    if (descr->pool->readable()) {
+    if (descr && descr->pool->readable()) {
         // NOTE: CT bit is inverted, to get the DMA buffer that's Not currently in use.
         size_t ct = ! hal_dma_get_ct(dma);
         descr->dmabuf[ct]->release();
diff --git a/src/DMABuffer.h b/src/DMABuffer.h
index 0f495df..50289fe 100644
--- a/src/DMABuffer.h
+++ b/src/DMABuffer.h
@@ -73,10 +73,8 @@ template <class T, size_t A=__SCB_DCACHE_LINE_SIZE> class DMABuffer {
         uint32_t flags;
 
     public:
-        DMABuffer *next;
-
         DMABuffer(Pool *pool=nullptr, size_t samples=0, size_t channels=0, T *mem=nullptr):
-            pool(pool), n_samples(samples), n_channels(channels), ptr(mem), ts(0), flags(0), next(nullptr) {
+            pool(pool), n_samples(samples), n_channels(channels), ptr(mem), ts(0), flags(0) {
         }
 
         T *data() {
@@ -158,36 +156,61 @@ template <class T, size_t A=__SCB_DCACHE_LINE_SIZE> class DMABufferPool {
     private:
         Queue<DMABuffer<T>*> wr_queue;
         Queue<DMABuffer<T>*> rd_queue;
-        std::unique_ptr<DMABuffer<T>[]> buffers;
         std::unique_ptr<uint8_t, decltype(&AlignedAlloc<A>::free)> pool;
 
     public:
         DMABufferPool(size_t n_samples, size_t n_channels, size_t n_buffers):
-            buffers(nullptr), pool(nullptr, AlignedAlloc<A>::free) {
+            wr_queue(n_buffers), rd_queue(n_buffers), pool(nullptr, AlignedAlloc<A>::free) {
             // Round up to next multiple of alignment.
             size_t bufsize = AlignedAlloc<A>::round(n_samples * n_channels * sizeof(T));
-            if (bufsize) {
-                // Allocate non-aligned memory for the DMA buffers objects.
-                buffers.reset(new DMABuffer<T>[n_buffers]);
-
-                // Allocate aligned memory pool for DMA buffers pointers.
+            if (bufsize && rd_queue && wr_queue) {
+                // Allocate an aligned memory pool for DMA buffers.
                 pool.reset((uint8_t *) AlignedAlloc<A>::malloc(n_buffers * bufsize));
-            }
-            if (buffers && pool) {
-                // Init DMA buffers using aligned pointers to dma buffers memory.
+                if (!pool) {
+                    // Failed to allocate memory pool.
+                    return;
+                }
+                // Allocate the DMA buffers, initialize them using aligned
+                // pointers from the pool, and add them to the ready queue.
                 for (size_t i=0; i<n_buffers; i++) {
-                    buffers[i] = DMABuffer<T>(this, n_samples, n_channels, (T *) &pool.get()[i * bufsize]);
-                    wr_queue.push(&buffers[i]);
+                    DMABuffer<T> *buf =  new DMABuffer<T>(
+                        this, n_samples, n_channels, (T *) &pool.get()[i * bufsize]
+                    );
+                    if (buf == nullptr) {
+                        break;
+                    }
+                    wr_queue.push(buf);
                 }
             }
         }
 
-        size_t writable() {
-            return wr_queue.size();
+        ~DMABufferPool() {
+            size_t count = 0;
+            DMABuffer<T> *buf = nullptr;
+
+            while (readable()) {
+                delete dequeue();
+                count ++;
+            }
+
+            while (writable()) {
+                delete allocate();
+                count ++;
+            }
+        }
+
+        bool writable() {
+            return !(wr_queue.empty());
         }
 
-        size_t readable() {
-            return rd_queue.size();
+        bool readable() {
+            return !(rd_queue.empty());
+        }
+
+        void flush() {
+            while (readable()) {
+                release(dequeue());
+            }
         }
 
         DMABuffer<T> *allocate() {
diff --git a/src/HALConfig.cpp b/src/HALConfig.cpp
index 7c2de12..225029a 100644
--- a/src/HALConfig.cpp
+++ b/src/HALConfig.cpp
@@ -74,7 +74,7 @@ int hal_dma_config(DMA_HandleTypeDef *dma, IRQn_Type irqn, uint32_t direction) {
 
     // DMA Init
     dma->Init.Mode                  = DMA_DOUBLE_BUFFER_M0;
-    dma->Init.Priority              = DMA_PRIORITY_LOW;
+    dma->Init.Priority              = DMA_PRIORITY_VERY_HIGH;
     dma->Init.Direction             = direction;
     dma->Init.FIFOMode              = DMA_FIFOMODE_ENABLE;
     dma->Init.FIFOThreshold         = DMA_FIFO_THRESHOLD_FULL;
diff --git a/src/Queue.h b/src/Queue.h
index de2dc1b..0b7e9f0 100644
--- a/src/Queue.h
+++ b/src/Queue.h
@@ -24,45 +24,58 @@
 
 template <class T> class Queue {
     private:
-        T head;
-        T tail;
-        size_t _size;
+        size_t capacity;
+        volatile size_t tail;
+        volatile size_t head;
+        std::unique_ptr<T[]> buff;
+
+    private:
+        inline size_t next_pos(size_t x) {
+            return (((x) + 1) % (capacity));
+        }
 
     public:
-        Queue(): head(nullptr), tail(nullptr), _size(0) {
+        Queue(size_t size=0):
+         capacity(size), tail(0), head(0), buff(nullptr) {
+            if (size) {
+                tail = head = 0;
+                capacity = size + 1;
+                buff.reset(new T[capacity]);
+            }
+        }
 
+        void reset() {
+            tail = head = 0;
         }
 
-        size_t size() {
-            return _size;
+        size_t empty() {
+            return tail == head;
         }
 
-        bool empty() {
-            return !_size;
+        operator bool() const {
+            return buff.get() != nullptr;
         }
 
-        void push(T value) {
-            _size++;
-            value->next = nullptr;
-            if (head == nullptr) {
-                head = value;
-            }
-            if (tail) {
-                tail->next = value;
+        bool push(T data) {
+            bool ret = false;
+            size_t next = next_pos(head);
+            if (buff && (next != tail)) {
+                buff[head] = data;
+                head = next;
+                ret = true;
             }
-            tail = value;
+            return ret;
         }
 
-        T pop() {
-            _size--;
-            T value = head;
-            if (head) {
-                head = head->next;
-            }
-            if (head == nullptr) {
-                tail = nullptr;
+        T pop(bool peek=false) {
+            if (buff && (tail != head)) {
+                T data = buff[tail];
+                if (!peek) {
+                    tail = next_pos(tail);
+                }
+                return data;
             }
-            return value;
+            return T();
         }
 };
 #endif //__QUEUE_H__