diff --git a/examples/Makefile b/examples/Makefile
index 4292cd6..0e74b74 100644
--- a/examples/Makefile
+++ b/examples/Makefile
@@ -7,7 +7,7 @@ INCLUDE_PATH	= ../include
 CFLAGS		+= -I$(INCLUDE_PATH)
 LDFLAGS		= -L../src -lskipstone
 
-EXAMPLES	= read music map
+EXAMPLES	= read music map test
 
 RM		= /bin/rm
 
diff --git a/examples/test.c b/examples/test.c
new file mode 100644
index 0000000..7caa4be
--- /dev/null
+++ b/examples/test.c
@@ -0,0 +1,53 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <skipstone/link.h>
+#include <skipstone/message.h>
+
+static void usage(int argc, char **argv) {
+    fprintf(stderr, "usage: %s /dev/rfcommX\n", argv[0]);
+    exit(1);
+}
+
+static int answer_phone_version_message(skipstone_message_service *service,
+        void *buf, uint16_t size, uint16_t id) {
+    uint8_t response[12] = {
+        0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02
+    };
+
+    skipstone_message_service_queue(service, &response, sizeof(response), id);
+
+    return 0;
+}
+
+int main(int argc, char **argv) {
+    skipstone_link *link;
+    skipstone_message_service *service;
+
+    if (argc != 2) {
+        usage(argc, argv);
+    }
+
+    if ((link = skipstone_link_open_serial(argv[1])) == NULL) {
+        goto error_link_open;
+    }
+
+    if ((service = skipstone_message_service_new()) == NULL) {
+        goto error_message_service_new;
+    }
+
+    skipstone_message_service_register(service, 17, answer_phone_version_message, NULL);
+
+    skipstone_message_service_run(service, link);
+
+    skipstone_link_close(link);
+
+    return 0;
+
+error_message_service_new:
+    skipstone_link_close(link);
+
+error_link_open:
+    return 1;
+}
diff --git a/examples/test.c-e b/examples/test.c-e
new file mode 100644
index 0000000..7caa4be
--- /dev/null
+++ b/examples/test.c-e
@@ -0,0 +1,53 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <skipstone/link.h>
+#include <skipstone/message.h>
+
+static void usage(int argc, char **argv) {
+    fprintf(stderr, "usage: %s /dev/rfcommX\n", argv[0]);
+    exit(1);
+}
+
+static int answer_phone_version_message(skipstone_message_service *service,
+        void *buf, uint16_t size, uint16_t id) {
+    uint8_t response[12] = {
+        0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02
+    };
+
+    skipstone_message_service_queue(service, &response, sizeof(response), id);
+
+    return 0;
+}
+
+int main(int argc, char **argv) {
+    skipstone_link *link;
+    skipstone_message_service *service;
+
+    if (argc != 2) {
+        usage(argc, argv);
+    }
+
+    if ((link = skipstone_link_open_serial(argv[1])) == NULL) {
+        goto error_link_open;
+    }
+
+    if ((service = skipstone_message_service_new()) == NULL) {
+        goto error_message_service_new;
+    }
+
+    skipstone_message_service_register(service, 17, answer_phone_version_message, NULL);
+
+    skipstone_message_service_run(service, link);
+
+    skipstone_link_close(link);
+
+    return 0;
+
+error_message_service_new:
+    skipstone_link_close(link);
+
+error_link_open:
+    return 1;
+}
diff --git a/src/Makefile b/src/Makefile
index f443df6..15b9839 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -7,9 +7,9 @@ CC		= $(CROSS)cc
 CFLAGS		+= -fPIC -I$(INCLUDE_PATH)
 LDFLAGS		=
 
-HEADERS		= watch.h link.h map.h message.h
+HEADERS		= watch.h link.h map.h message.h queue.h
 
-OBJS		= watch.o link.o map.o message.o
+OBJS		= watch.o link.o map.o message.o queue.o
 
 VERSION_MAJOR	= 0
 VERSION_MINOR	= 0.1
diff --git a/src/queue.c b/src/queue.c
index f9281d2..6c6e39a 100644
--- a/src/queue.c
+++ b/src/queue.c
@@ -35,7 +35,7 @@ size_t skipstone_queue_count(skipstone_queue *queue) {
 int skipstone_queue_push(skipstone_queue *queue, void *value) {
     struct entry *next;
 
-    if ((next = malloc(sizeof(*entry))) == NULL) {
+    if ((next = malloc(sizeof(*next))) == NULL) {
         goto error_malloc_next;
     }
 
diff --git a/src/queue.c-e b/src/queue.c-e
index f9281d2..6c6e39a 100644
--- a/src/queue.c-e
+++ b/src/queue.c-e
@@ -35,7 +35,7 @@ size_t skipstone_queue_count(skipstone_queue *queue) {
 int skipstone_queue_push(skipstone_queue *queue, void *value) {
     struct entry *next;
 
-    if ((next = malloc(sizeof(*entry))) == NULL) {
+    if ((next = malloc(sizeof(*next))) == NULL) {
         goto error_malloc_next;
     }