1 files changed, 2 insertions, 7 deletions

diff --git a/src/mailbox.c b/src/mailbox.c
index aacb747..310b2b1 100644
--- a/src/mailbox.c
+++ b/src/mailbox.c
@@ -16,13 +16,8 @@ static inline uint8_t msg_channel(Message msg) {
   return msg & 0xf;
 }
 
-static inline const void* msg_data(Message msg) {
-  // The data field is actually a pointer to the data.
-  return (const void*)((uintptr_t)(msg >> 4));
-}
-
 static inline Message msg_make(uint8_t channel, volatile const void* data) {
-  return ((uintptr_t)(data) << 4) | (channel & 0xf);
+  return ((uintptr_t)(data) & ~0xf) | (channel & 0xf);
 }
 
 typedef struct Mailbox {
@@ -61,7 +56,7 @@ const Mail* mbox_read(uint8_t channel) {
     // Read the mail.
     msg = pMailbox->inbox;
   } while (msg_channel(msg) != channel);
-  return (const Mail*)(msg_data(msg));
+  return (const Mail*)((uintptr_t)msg & ~0xf);
 }
 
 void mbox_write(uint8_t channel, volatile const void* mail) {