diff --git a/src/msg_queue.c b/src/msg_queue.c
index 6d46c84..caa5afe 100644
--- a/src/msg_queue.c
+++ b/src/msg_queue.c
@@ -28,7 +28,7 @@ void MqDestroy(PMSG_QUEUE queue)
   free(queue);
 }
 
-static void post_internal(MSG_QUEUE volatile *queue, PMSG msg)
+static void post_internal(PMSG_QUEUE queue, PMSG msg)
 {
   PMSG nexttail;
   
@@ -46,7 +46,7 @@ static void post_internal(MSG_QUEUE volatile *queue, PMSG msg)
   pthread_mutex_unlock(&(queue->mutex));
 }
 
-void MqPost(MSG_QUEUE volatile *queue, uintptr_t target, unsigned message, const uintptr_t *attrs, int nattrs)
+void MqPost(PMSG_QUEUE queue, uintptr_t target, unsigned message, const uintptr_t *attrs, int nattrs)
 {
   MSG tmpmsg;
 
@@ -61,7 +61,7 @@ void MqPost(MSG_QUEUE volatile *queue, uintptr_t target, unsigned message, const
   post_internal(queue, &tmpmsg);
 }
 
-void MqPost1(MSG_QUEUE volatile *queue, uintptr_t target, unsigned message, uintptr_t attr1)
+void MqPost1(PMSG_QUEUE queue, uintptr_t target, unsigned message, uintptr_t attr1)
 {
   MSG tmpmsg;
   
@@ -73,7 +73,7 @@ void MqPost1(MSG_QUEUE volatile *queue, uintptr_t target, unsigned message, uint
   post_internal(queue, &tmpmsg);
 }
 
-int MqPeek(MSG_QUEUE volatile *queue, PMSG msg, unsigned flags)
+int MqPeek(PMSG_QUEUE queue, PMSG msg, unsigned flags)
 {
   int rc = 0;
   PMSG nexthead;
@@ -87,6 +87,7 @@ int MqPeek(MSG_QUEUE volatile *queue, PMSG msg, unsigned flags)
     memcpy(msg, queue->head, sizeof(MSG));
     if (flags & PEEK_REMOVE)
       queue->head = nexthead;
+    rc = 1;
   }
   pthread_mutex_unlock(&(queue->mutex));
   return rc;
diff --git a/src/msg_queue.h b/src/msg_queue.h
index fd44f0f..4b0ddc5 100644
--- a/src/msg_queue.h
+++ b/src/msg_queue.h
@@ -20,9 +20,8 @@ typedef struct tagMSG_QUEUE {
 
 extern PMSG_QUEUE MqAlloc(int nentries);
 extern void MqDestroy(PMSG_QUEUE queue);
-extern void MqPost(MSG_QUEUE volatile *queue, uintptr_t target, unsigned message, const uintptr_t *attrs,
-		   int nattrs);
-extern void MqPost1(MSG_QUEUE volatile *queue, uintptr_t target, unsigned message, uintptr_t attr1);
-extern int MqPeek(MSG_QUEUE volatile *queue, PMSG msg, unsigned flags);
+extern void MqPost(PMSG_QUEUE queue, uintptr_t target, unsigned message, const uintptr_t *attrs, int nattrs);
+extern void MqPost1(PMSG_QUEUE queue, uintptr_t target, unsigned message, uintptr_t attr1);
+extern int MqPeek(PMSG_QUEUE queue, PMSG msg, unsigned flags);
 
 #endif /* __MSG_QUEUE_H_INCLUDED */
diff --git a/src/sysinput.c b/src/sysinput.c
index 055a86f..c45d6f7 100644
--- a/src/sysinput.c
+++ b/src/sysinput.c
@@ -5,7 +5,7 @@
 #include "msg_queue.h"
 #include "gpio.h"
 
-MSG_QUEUE volatile *Sys_Queue = NULL;
+PMSG_QUEUE Sys_Queue = NULL;
 
 static pthread_t ithread;
 static volatile sig_atomic_t running = 1;
diff --git a/src/sysinput.h b/src/sysinput.h
index 7142f4b..29b5a18 100644
--- a/src/sysinput.h
+++ b/src/sysinput.h
@@ -3,7 +3,7 @@
 
 #include "msg_queue.h"
 
-extern MSG_QUEUE volatile *Sys_Queue;
+extern PMSG_QUEUE Sys_Queue;
 
 extern int SysEnableInput(void);
 extern void SysDisableInput(void);