diff --git a/include/ringbuf.h b/include/ringbuf.h
index 61bfae9..ffe0159 100644
--- a/include/ringbuf.h
+++ b/include/ringbuf.h
@@ -63,6 +63,9 @@ extern "C" {
     volatile uint8_t *Ringbuf_Peek(RING_BUFFER const *b);
     bool Ringbuf_Pop(RING_BUFFER * b,
         uint8_t * data_element);
+    bool Ringbuf_Pop_Element(RING_BUFFER * b,
+        uint8_t * this_element,
+        uint8_t * data_element);
     bool Ringbuf_Put_Front(RING_BUFFER * b,
         uint8_t * data_element);
     /* head */
@@ -70,6 +73,8 @@ extern "C" {
         uint8_t * data_element);
     /* pair of functions to use head memory directly */
     volatile uint8_t *Ringbuf_Data_Peek(RING_BUFFER * b);
+    volatile uint8_t *Ringbuf_Peek_Next(RING_BUFFER const *b,
+        uint8_t * data_element);
     bool Ringbuf_Data_Put(RING_BUFFER * b, volatile uint8_t *data_element);
     /* Note: element_count must be a power of two */
     bool Ringbuf_Init(RING_BUFFER * b,
diff --git a/ports/linux/dlmstp_linux.c b/ports/linux/dlmstp_linux.c
index 3c256c1..007c5b5 100644
--- a/ports/linux/dlmstp_linux.c
+++ b/ports/linux/dlmstp_linux.c
@@ -581,7 +581,21 @@ uint16_t MSTP_Get_Reply(
         mstp_port->DataLength, mstp_port->SourceAddress,
         (uint8_t *) & pkt->buffer[0], pkt->length, pkt->destination_mac);
     if (!matched) {
-        return 0;
+        /* Walk the rest of the ring buffer to see if we can find a match */
+        while (!matched &&
+               (pkt = (struct mstp_pdu_packet *)Ringbuf_Peek_Next(&poSharedData->PDU_Queue, (uint8_t *)pkt)) != NULL) {
+            matched =
+                dlmstp_compare_data_expecting_reply(&mstp_port->InputBuffer[0],
+                                                    mstp_port->DataLength,
+                                                    mstp_port->SourceAddress,
+                                                    (uint8_t *) & pkt->buffer[0],
+                                                    pkt->length,
+                                                    pkt->destination_mac);
+        }
+        if (!matched) {
+            /* Still didn't find a match so just bail out */
+            return 0;
+        }
     }
     if (pkt->data_expecting_reply) {
         frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
@@ -592,7 +606,8 @@ uint16_t MSTP_Get_Reply(
     pdu_len = MSTP_Create_Frame(&mstp_port->OutputBuffer[0],    /* <-- loading this */
         mstp_port->OutputBufferSize, frame_type, pkt->destination_mac,
         mstp_port->This_Station, (uint8_t *) & pkt->buffer[0], pkt->length);
-    (void) Ringbuf_Pop(&poSharedData->PDU_Queue, NULL);
+    /* This will pop the element no matter where we found it */
+    (void) Ringbuf_Pop_Element(&poSharedData->PDU_Queue, (uint8_t *)pkt, NULL);
 
     return pdu_len;
 }
diff --git a/src/ringbuf.c b/src/ringbuf.c
index 8cb0786..b032d6d 100644
--- a/src/ringbuf.c
+++ b/src/ringbuf.c
@@ -180,6 +180,37 @@ volatile uint8_t *Ringbuf_Peek(RING_BUFFER const *b)
     return data_element;
 }
 
+/**
+* Looks at the data from the next element of the list without removing it
+*
+* @param  b - pointer to RING_BUFFER structure
+* @param  data_element - find the next element from this one
+* @return pointer to the data, or NULL if nothing in the list
+*/
+volatile uint8_t *Ringbuf_Peek_Next(RING_BUFFER const *b,
+                                    uint8_t * data_element)
+{
+    unsigned index;    /* list index */
+    volatile uint8_t *this_element;
+    volatile uint8_t *next_element = NULL;      /* return value */
+    if (!Ringbuf_Empty(b) && data_element != NULL) {
+        /* Use (b->head-1) here to avoid walking off end of ring */
+        for (index = b->tail; index < b->head-1; index++) {
+            /* Find the specified data_element */
+            this_element = b->buffer +
+                           ((index % b->element_count) * b->element_size);
+            if (data_element == this_element) {
+                /* Found the current element, get the next one on the list */
+                next_element = b->buffer +
+                           (((index+1) % b->element_count) * b->element_size);
+                break;
+            }
+        }
+    }
+
+    return next_element;
+}
+
 /**
 * Copy the data from the front of the list, and removes it
 *
@@ -209,6 +240,61 @@ bool Ringbuf_Pop(RING_BUFFER * b,
     return status;
 }
 
+/**
+* Copy the data from the specified element, and removes it and moves other
+* elements up the list
+*
+* @param  b - pointer to RING_BUFFER structure
+* @param  this_element - element to find
+* @param  data_element - element of data that is loaded with data from ring
+* @return true if data was copied, false if list is empty
+*/
+bool Ringbuf_Pop_Element(RING_BUFFER * b,
+                         uint8_t * this_element,
+                         uint8_t * data_element)
+{
+    bool status = false;        /* return value */
+    volatile uint8_t *ring_data = NULL; /* used to help point ring data */
+    volatile uint8_t *prev_data;
+    unsigned index;  /* list index */
+    unsigned this_index = b->head;  /* index of element to remove */
+    unsigned i; /* loop counter */
+    if (!Ringbuf_Empty(b) && this_element != NULL) {
+        for (index = b->tail; index < b->head; index++) {
+            /* Find the specified data_element */
+            ring_data = b->buffer +
+                        ((index % b->element_count) * b->element_size);
+            if (this_element == ring_data) {
+                /* Found the specified element, copy the data if required */
+                this_index = index;
+                if (data_element) {
+                    for (i = 0; i < b->element_size; i++) {
+                        data_element[i] = ring_data[i];
+                    }
+                }
+                break;
+            }
+        }
+        if (this_index < b->head) {
+            /* Found a match, move elements up the list to fill the gap */
+            for ( index = this_index; index > b->tail; index--) {
+                /* Get pointers to current and previous data_elements */
+                ring_data = b->buffer +
+                            ((index % b->element_count) * b->element_size);
+                prev_data = b->buffer +
+                            (((index-1) % b->element_count) * b->element_size);
+                for (i = 0; i < b->element_size; i++) {
+                    ring_data[i] = prev_data[i];
+                }
+            }
+        }
+        b->tail++;
+        status = true;
+    }
+
+    return status;
+}
+
 /**
 * Adds an element of data to the ring buffer
 *
@@ -320,7 +406,7 @@ bool Ringbuf_Data_Put(RING_BUFFER * b, volatile uint8_t *data_element)
             if (ring_data == data_element) {
                 /* same chunk of memory - okay to signal the head */
                 b->head++;
-            Ringbuf_Depth_Update(b);
+                Ringbuf_Depth_Update(b);
                 status = true;
             }
         }
@@ -589,6 +675,126 @@ void testRingBufPowerOfTwo(Test * pTest)
     ct_test(pTest, NEXT_POWER_OF_2(500)==512);
 }
 
+/**
+* Unit Test for the ring buffer peek/pop next element
+*
+* @param  pTest - test tracking pointer
+* @param  data_store - buffer to store elements
+* @param  data_element - one data element
+* @param  element_size - size of one data element
+* @param  element_count - number of data elements in the store
+*/
+static bool testRingBufNextElement(Test * pTest,
+    uint8_t * data_store,
+    uint8_t * data_element,
+    unsigned element_size,
+    unsigned element_count)
+{
+    RING_BUFFER test_buffer;
+    volatile uint8_t *test_data;
+    unsigned index;
+    unsigned data_index;
+    bool status;
+    status = Ringbuf_Init(&test_buffer, data_store,
+        element_size, element_count);
+    if (!status) {
+        return false;
+    }
+    ct_test(pTest, Ringbuf_Empty(&test_buffer));
+    ct_test(pTest, Ringbuf_Depth(&test_buffer) == 0);
+
+    for (data_index = 0; data_index < element_size; data_index++) {
+        data_element[data_index] = data_index;
+    }
+    status = Ringbuf_Put(&test_buffer, data_element);
+    ct_test(pTest, status == true);
+    ct_test(pTest, !Ringbuf_Empty(&test_buffer));
+    ct_test(pTest, Ringbuf_Depth(&test_buffer) == 1);
+
+    test_data = Ringbuf_Peek(&test_buffer);
+    for (data_index = 0; data_index < element_size; data_index++) {
+        ct_test(pTest, test_data[data_index] == data_element[data_index]);
+    }
+    ct_test(pTest, !Ringbuf_Empty(&test_buffer));
+    (void) Ringbuf_Pop(&test_buffer, NULL);
+    ct_test(pTest, Ringbuf_Empty(&test_buffer));
+    ct_test(pTest, Ringbuf_Depth(&test_buffer) == 1);
+
+    /* fill to max */
+    for (index = 0; index < element_count; index++) {
+        for (data_index = 0; data_index < element_size; data_index++) {
+            data_element[data_index] = index;
+        }
+        status = Ringbuf_Put(&test_buffer, data_element);
+        ct_test(pTest, status == true);
+        ct_test(pTest, !Ringbuf_Empty(&test_buffer));
+        ct_test(pTest, Ringbuf_Depth(&test_buffer) == (index+1));
+    }
+    ct_test(pTest, Ringbuf_Depth(&test_buffer) == element_count);
+    ct_test(pTest, Ringbuf_Count(&test_buffer) == element_count);
+
+    /* Walk through ring buffer */
+    test_data = Ringbuf_Peek(&test_buffer);
+    ct_test(pTest, test_data);
+    for (index = 1; index < element_count; index++) {
+        test_data = Ringbuf_Peek_Next(&test_buffer, (uint8_t *)test_data);
+        ct_test(pTest, test_data);
+        if (test_data) {
+            for (data_index = 0; data_index < element_size; data_index++) {
+                ct_test(pTest, test_data[data_index] == index);
+            }
+        }
+    }
+    ct_test(pTest, Ringbuf_Count(&test_buffer) == element_count);
+    /* Try to walk off end of buffer - should return NULL */
+    test_data = Ringbuf_Peek_Next(&test_buffer, (uint8_t *)test_data);
+    ct_test(pTest, (test_data == NULL));
+
+    /* Walk through ring buffer and pop alternate elements */
+    test_data = Ringbuf_Peek(&test_buffer);
+    ct_test(pTest, test_data);
+    for (index = 1; index < element_count/2; index++) {
+        test_data = Ringbuf_Peek_Next(&test_buffer, (uint8_t *)test_data);
+        ct_test(pTest, test_data);
+        (void) Ringbuf_Pop_Element(&test_buffer, (uint8_t *)test_data, NULL);
+        test_data = Ringbuf_Peek_Next(&test_buffer, (uint8_t *)test_data);
+    }
+    ct_test(pTest, Ringbuf_Count(&test_buffer) == element_count/2+1);
+
+    /* Walk through ring buffer and check data */
+    test_data = Ringbuf_Peek(&test_buffer);
+    ct_test(pTest, test_data);
+    for (index = 0; index < element_count/2; index++) {
+        if (test_data) {
+            for (data_index = 0; data_index < element_size; data_index++) {
+                ct_test(pTest, test_data[data_index] == index*2);
+            }
+        }
+        test_data = Ringbuf_Peek_Next(&test_buffer, (uint8_t *)test_data);
+        ct_test(pTest, test_data);
+    }
+    ct_test(pTest, Ringbuf_Count(&test_buffer) == element_count/2+1);
+
+    return true;
+}
+
+/**
+* Unit Test for the ring buffer with 16 data elements
+*
+* @param       pTest - test tracking pointer
+*/
+void testRingBufNextElementSizeSmall(Test * pTest)
+{
+    bool status;
+    uint8_t data_element[5];
+    uint8_t data_store[sizeof(data_element) * NEXT_POWER_OF_2(16)];
+
+    status = testRingBufNextElement(pTest, data_store, data_element,
+        sizeof(data_element),
+        sizeof(data_store) / sizeof(data_element));
+    ct_test(pTest, status);
+}
+
 #ifdef TEST_RING_BUFFER
 /**
 * Main program entry for Unit Test
@@ -611,6 +817,8 @@ int main(void)
     assert(rc);
     rc = ct_addTestFunction(pTest, testRingBufSizeInvalid);
     assert(rc);
+    rc = ct_addTestFunction(pTest, testRingBufNextElementSizeSmall);
+    assert(rc);
 
     ct_setStream(pTest, stdout);
     ct_run(pTest);