check timeout on delivery

prrt/proto/stores/packetDeliveryStore.c

@@ -99,22 +99,32 @@ PrrtPacket *PrrtPacketDeliveryStore_get_packet_timedwait(PrrtPacketDeliveryStore
 
 PrrtPacket *PrrtPacketDeliveryStore_get_packet(PrrtPacketDeliveryStore *store, prrtTimestamp_t start, prrtTimestamp_t stop) {
     PrrtPacket *packet = NULL;
-
+    bool timed_out;
+    
     check(pthread_mutex_lock(&store->lock) == EXIT_SUCCESS, "Lock failed.");
-
-    if (start > stop) {
-        packet = BPTree_get_first_in_range(store->tree, (BPTreeKey_t) start, TIMESTAMP_SPACE - 1);
-        if (packet == NULL) {
-            packet = BPTree_get_first_in_range(store->tree, 0, (BPTreeKey_t) stop);
+    do {
+        timed_out = false;
+        
+        if (start > stop) {
+            packet = BPTree_get_first_in_range(store->tree, (BPTreeKey_t) start, TIMESTAMP_SPACE - 1);
+            if (packet == NULL) {
+                packet = BPTree_get_first_in_range(store->tree, 0, (BPTreeKey_t) stop);
+            }
+        } else {
+            packet = BPTree_get_first_in_range(store->tree, (BPTreeKey_t) start, (BPTreeKey_t) stop);
         }
-    } else {
-        packet = BPTree_get_first_in_range(store->tree, (BPTreeKey_t) start, (BPTreeKey_t) stop);
-    }
-
-    if(packet != NULL) {
-        store->tree = BPTree_delete(store->tree, PrrtDataPacket_packet_timeout(packet));
-    }
-
+        
+        if(packet != NULL) {
+            store->tree = BPTree_delete(store->tree, PrrtDataPacket_packet_timeout(packet));
+            prrtTimestamp_t now = PrrtClock_get_current_time_us();
+            if (PrrtTimestamp_cmp(now, PrrtDataPacket_packet_timeout(packet)) > 0) {
+                timed_out = true;
+                PrrtPacket_destroy(packet);
+                packet = NULL;
+            }
+        }
+        
+    } while (timed_out);
     check(pthread_mutex_unlock(&store->lock) == EXIT_SUCCESS, "Unlock failed.");
 
     return packet;