diff --git a/examples/test.c b/examples/test.c
index 7caa4be..4a37a9f 100644
--- a/examples/test.c
+++ b/examples/test.c
@@ -16,6 +16,9 @@ static int answer_phone_version_message(skipstone_message_service *service,
         0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02
     };
 
+    printf("Sending %hu message to %hu in response to %hu message\n",
+        sizeof(response), id, size);
+
     skipstone_message_service_queue(service, &response, sizeof(response), id);
 
     return 0;
@@ -37,6 +40,8 @@ int main(int argc, char **argv) {
         goto error_message_service_new;
     }
 
+    printf("Registering endpoint %p\n", answer_phone_version_message);
+
     skipstone_message_service_register(service, 17, answer_phone_version_message, NULL);
 
     skipstone_message_service_run(service, link);
diff --git a/examples/test.c-e b/examples/test.c-e
index 7caa4be..4a37a9f 100644
--- a/examples/test.c-e
+++ b/examples/test.c-e
@@ -16,6 +16,9 @@ static int answer_phone_version_message(skipstone_message_service *service,
         0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02
     };
 
+    printf("Sending %hu message to %hu in response to %hu message\n",
+        sizeof(response), id, size);
+
     skipstone_message_service_queue(service, &response, sizeof(response), id);
 
     return 0;
@@ -37,6 +40,8 @@ int main(int argc, char **argv) {
         goto error_message_service_new;
     }
 
+    printf("Registering endpoint %p\n", answer_phone_version_message);
+
     skipstone_message_service_register(service, 17, answer_phone_version_message, NULL);
 
     skipstone_message_service_run(service, link);
diff --git a/src/message.c b/src/message.c
index 8efb14f..fd01d0b 100644
--- a/src/message.c
+++ b/src/message.c
@@ -1,4 +1,3 @@
-#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 
@@ -80,8 +79,15 @@ int skipstone_message_service_queue(skipstone_message_service *service,
 
     memcpy(message + 1, buf, size);
 
+    if (skipstone_queue_push(service->pending, message) < 0) {
+        goto error_queue_push_pending;
+    }
+
     return 0;
 
+error_queue_push_pending:
+    free(message);
+
 error_malloc_message:
     return -1;
 }
@@ -95,16 +101,16 @@ int skipstone_message_service_run(skipstone_message_service *service,
     }
 
     while (1) {
-        uint16_t size, id;
-        skipstone_message_handler *handler;
         skipstone_message_header *message;
+        struct endpoint *endpoint;
+        uint16_t size, id;
 
         if (skipstone_link_recv(link, buf, &size, &id) < 0) {
             goto error_io;
         }
 
-        if ((handler = skipstone_map_get(service->endpoints, id)) != NULL) {
-            if (handler(service, buf, size, id) < 0) {
+        if ((endpoint = skipstone_map_get(service->endpoints, id)) != NULL) {
+            if (endpoint->handler(service, buf, size, id) < 0) {
                 goto error_io;
             }
         }
diff --git a/src/message.c-e b/src/message.c-e
index 8efb14f..fd01d0b 100644
--- a/src/message.c-e
+++ b/src/message.c-e
@@ -1,4 +1,3 @@
-#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 
@@ -80,8 +79,15 @@ int skipstone_message_service_queue(skipstone_message_service *service,
 
     memcpy(message + 1, buf, size);
 
+    if (skipstone_queue_push(service->pending, message) < 0) {
+        goto error_queue_push_pending;
+    }
+
     return 0;
 
+error_queue_push_pending:
+    free(message);
+
 error_malloc_message:
     return -1;
 }
@@ -95,16 +101,16 @@ int skipstone_message_service_run(skipstone_message_service *service,
     }
 
     while (1) {
-        uint16_t size, id;
-        skipstone_message_handler *handler;
         skipstone_message_header *message;
+        struct endpoint *endpoint;
+        uint16_t size, id;
 
         if (skipstone_link_recv(link, buf, &size, &id) < 0) {
             goto error_io;
         }
 
-        if ((handler = skipstone_map_get(service->endpoints, id)) != NULL) {
-            if (handler(service, buf, size, id) < 0) {
+        if ((endpoint = skipstone_map_get(service->endpoints, id)) != NULL) {
+            if (endpoint->handler(service, buf, size, id) < 0) {
                 goto error_io;
             }
         }
diff --git a/src/queue.c b/src/queue.c
index 8935a29..aef0dfe 100644
--- a/src/queue.c
+++ b/src/queue.c
@@ -61,6 +61,7 @@ int skipstone_queue_push(skipstone_queue *queue, void *value) {
     }
 
     queue->last = next;
+    queue->count++;
 
     return 0;
 
@@ -71,13 +72,13 @@ error_malloc_next:
 int skipstone_queue_pop(skipstone_queue *queue, void **value) {
     struct entry *last = queue->last;
 
-    if (last == NULL) {
+    if (queue->count == 0) {
         return 0;
     }
 
-    *value = last->value;
-
+    *value      = last->value;
     queue->last = last->prev;
+    queue->count--;
 
     free(last);
 
diff --git a/src/queue.c-e b/src/queue.c-e
index 8935a29..aef0dfe 100644
--- a/src/queue.c-e
+++ b/src/queue.c-e
@@ -61,6 +61,7 @@ int skipstone_queue_push(skipstone_queue *queue, void *value) {
     }
 
     queue->last = next;
+    queue->count++;
 
     return 0;
 
@@ -71,13 +72,13 @@ error_malloc_next:
 int skipstone_queue_pop(skipstone_queue *queue, void **value) {
     struct entry *last = queue->last;
 
-    if (last == NULL) {
+    if (queue->count == 0) {
         return 0;
     }
 
-    *value = last->value;
-
+    *value      = last->value;
     queue->last = last->prev;
+    queue->count--;
 
     free(last);