diff --git a/include/skipstone/message.h b/include/skipstone/message.h
index 261c0d0..208a336 100644
--- a/include/skipstone/message.h
+++ b/include/skipstone/message.h
@@ -19,18 +19,26 @@ enum {
     SKIPSTONE_MESSAGE_ENDPOINT_PHONE_CONTROL  = 33
 };
 
+typedef struct _skipstone_message_header {
+    uint16_t size,
+             id;
+} skipstone_message_header;
+
 typedef struct _skipstone_message_service skipstone_message_service;
 
-typedef int (*skipstone_message_handler)(skipstone_message_service *service,
-    void *buf, uint16_t size, uint16_t endpoint);
+typedef int (skipstone_message_handler)(skipstone_message_service *service,
+    void *buf, uint16_t size, uint16_t id);
 
 skipstone_message_service *skipstone_message_service_new();
 
 int skipstone_message_service_register(skipstone_message_service *service,
-        uint16_t endpoint, skipstone_message_handler *handler, void *context);
+        uint16_t id, skipstone_message_handler *handler, void *context);
 
 int skipstone_message_service_deregister(skipstone_message_service *service,
-        uint16_t endpoint);
+        uint16_t id);
+
+int skipstone_message_service_queue(skipstone_message_service *service,
+        void *buf, uint16_t size, uint16_t id);
 
 int skipstone_message_service_run(skipstone_message_service *service,
         skipstone_link *link);
diff --git a/src/link.c b/src/link.c
index 280043d..debb669 100644
--- a/src/link.c
+++ b/src/link.c
@@ -15,11 +15,6 @@ enum skipstone_link_type {
     SKIPSTONE_WATCH_LINK_SERIAL = 1
 };
 
-struct message_header {
-    uint16_t size,
-             endpoint;
-};
-
 struct _skipstone_link {
     enum skipstone_link_type type;
 
@@ -104,8 +99,8 @@ int skipstone_link_close(skipstone_link *link) {
     return -1;
 }
 
-int skipstone_link_send(skipstone_link *link, void *buf, uint16_t size, uint16_t endpoint) {
-    struct message_header header;
+int skipstone_link_send(skipstone_link *link, void *buf, uint16_t size, uint16_t id) {
+    skipstone_message_header header;
 
     ssize_t wrlen;
 
@@ -116,8 +111,8 @@ int skipstone_link_send(skipstone_link *link, void *buf, uint16_t size, uint16_t
         goto error_toobig;
     }
 
-    header.size     = htobe16(size);
-    header.endpoint = htobe16(endpoint);
+    header.size = htobe16(size);
+    header.id   = htobe16(id);
 
     if ((wrlen = write(link->serial.fd, &header, sizeof(header))) < 0) {
         goto error_io;
@@ -139,8 +134,8 @@ error_toobig:
     return -1;
 }
 
-int skipstone_link_recv(skipstone_link *link, void *buf, uint16_t *size, uint16_t *endpoint) {
-    struct message_header header;
+int skipstone_link_recv(skipstone_link *link, void *buf, uint16_t *size, uint16_t *id) {
+    skipstone_message_header header;
 
     ssize_t len_read;
      size_t len_wanted, offset = 0;
@@ -164,8 +159,8 @@ int skipstone_link_recv(skipstone_link *link, void *buf, uint16_t *size, uint16_
         offset     += len_read;
     }
 
-    *size     = be16toh(header.size);
-    *endpoint = be16toh(header.endpoint);
+    *size = be16toh(header.size);
+    *id   = be16toh(header.id);
 
     return 0;
 
diff --git a/src/link.c-e b/src/link.c-e
index 280043d..debb669 100644
--- a/src/link.c-e
+++ b/src/link.c-e
@@ -15,11 +15,6 @@ enum skipstone_link_type {
     SKIPSTONE_WATCH_LINK_SERIAL = 1
 };
 
-struct message_header {
-    uint16_t size,
-             endpoint;
-};
-
 struct _skipstone_link {
     enum skipstone_link_type type;
 
@@ -104,8 +99,8 @@ int skipstone_link_close(skipstone_link *link) {
     return -1;
 }
 
-int skipstone_link_send(skipstone_link *link, void *buf, uint16_t size, uint16_t endpoint) {
-    struct message_header header;
+int skipstone_link_send(skipstone_link *link, void *buf, uint16_t size, uint16_t id) {
+    skipstone_message_header header;
 
     ssize_t wrlen;
 
@@ -116,8 +111,8 @@ int skipstone_link_send(skipstone_link *link, void *buf, uint16_t size, uint16_t
         goto error_toobig;
     }
 
-    header.size     = htobe16(size);
-    header.endpoint = htobe16(endpoint);
+    header.size = htobe16(size);
+    header.id   = htobe16(id);
 
     if ((wrlen = write(link->serial.fd, &header, sizeof(header))) < 0) {
         goto error_io;
@@ -139,8 +134,8 @@ error_toobig:
     return -1;
 }
 
-int skipstone_link_recv(skipstone_link *link, void *buf, uint16_t *size, uint16_t *endpoint) {
-    struct message_header header;
+int skipstone_link_recv(skipstone_link *link, void *buf, uint16_t *size, uint16_t *id) {
+    skipstone_message_header header;
 
     ssize_t len_read;
      size_t len_wanted, offset = 0;
@@ -164,8 +159,8 @@ int skipstone_link_recv(skipstone_link *link, void *buf, uint16_t *size, uint16_
         offset     += len_read;
     }
 
-    *size     = be16toh(header.size);
-    *endpoint = be16toh(header.endpoint);
+    *size = be16toh(header.size);
+    *id   = be16toh(header.id);
 
     return 0;
 
diff --git a/src/message.c b/src/message.c
index 8ee7372..f9754fc 100644
--- a/src/message.c
+++ b/src/message.c
@@ -1,5 +1,6 @@
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <skipstone/link.h>
 #include <skipstone/map.h>
@@ -43,22 +44,60 @@ int skipstone_message_service_deregister(skipstone_message_service *service, uin
     return skipstone_map_set((skipstone_map *)service, id, NULL);
 }
 
+int skipstone_message_service_queue(skipstone_message_service *service,
+        void *buf, uint16_t size, uint16_t id) {
+    skipstone_message_header *message;
+
+    if ((message = malloc(sizeof(*message) + size)) == NULL) {
+        goto error_malloc_message;
+    }
+
+    message->size = size;
+    message->id   = id;
+
+    memcpy(message + 1, buf, size);
+
+    return 0;
+
+error_malloc_message:
+    return -1;
+}
+
 int skipstone_message_service_run(skipstone_message_service *service,
         skipstone_link *link) {
     void *buf;
-    uint16_t len, id;
 
     if ((buf = malloc(SKIPSTONE_MESSAGE_MAX_PAYLOAD)) == NULL) {
         goto error_malloc_buf;
     }
 
-    while (skipstone_link_recv(link, buf, &len, &id) >= 0) {
-        printf("Received message %hu bytes for endpoint %hu\n",
-            len, id);
+    while (1) {
+        uint16_t size, id;
+        skipstone_message_handler *handler;
+        skipstone_message_header *message;
+
+        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) {
+                goto error_io;
+            }
+        }
+
+        while (skipstone_queue_pop(service->pending, (void **)&message)) {
+            if (skipstone_link_send(link, message + 1, message->size, message->id) < 0) {
+                goto error_io;
+            }
+        }
     }
 
     return 0;
 
+error_io:
+    free(buf);
+
 error_malloc_buf:
     return -1;
 }
diff --git a/src/message.c-e b/src/message.c-e
index 8ee7372..f9754fc 100644
--- a/src/message.c-e
+++ b/src/message.c-e
@@ -1,5 +1,6 @@
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <skipstone/link.h>
 #include <skipstone/map.h>
@@ -43,22 +44,60 @@ int skipstone_message_service_deregister(skipstone_message_service *service, uin
     return skipstone_map_set((skipstone_map *)service, id, NULL);
 }
 
+int skipstone_message_service_queue(skipstone_message_service *service,
+        void *buf, uint16_t size, uint16_t id) {
+    skipstone_message_header *message;
+
+    if ((message = malloc(sizeof(*message) + size)) == NULL) {
+        goto error_malloc_message;
+    }
+
+    message->size = size;
+    message->id   = id;
+
+    memcpy(message + 1, buf, size);
+
+    return 0;
+
+error_malloc_message:
+    return -1;
+}
+
 int skipstone_message_service_run(skipstone_message_service *service,
         skipstone_link *link) {
     void *buf;
-    uint16_t len, id;
 
     if ((buf = malloc(SKIPSTONE_MESSAGE_MAX_PAYLOAD)) == NULL) {
         goto error_malloc_buf;
     }
 
-    while (skipstone_link_recv(link, buf, &len, &id) >= 0) {
-        printf("Received message %hu bytes for endpoint %hu\n",
-            len, id);
+    while (1) {
+        uint16_t size, id;
+        skipstone_message_handler *handler;
+        skipstone_message_header *message;
+
+        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) {
+                goto error_io;
+            }
+        }
+
+        while (skipstone_queue_pop(service->pending, (void **)&message)) {
+            if (skipstone_link_send(link, message + 1, message->size, message->id) < 0) {
+                goto error_io;
+            }
+        }
     }
 
     return 0;
 
+error_io:
+    free(buf);
+
 error_malloc_buf:
     return -1;
 }
diff --git a/src/queue.c b/src/queue.c
index 6d98d51..f9281d2 100644
--- a/src/queue.c
+++ b/src/queue.c
@@ -63,5 +63,5 @@ int skipstone_queue_pop(skipstone_queue *queue, void **value) {
 
     free(last);
 
-    return 0;
+    return 1;
 }
diff --git a/src/queue.c-e b/src/queue.c-e
index 6d98d51..f9281d2 100644
--- a/src/queue.c-e
+++ b/src/queue.c-e
@@ -63,5 +63,5 @@ int skipstone_queue_pop(skipstone_queue *queue, void **value) {
 
     free(last);
 
-    return 0;
+    return 1;
 }