4 files changed, 30 insertions, 20 deletions

diff --git a/src/framebuffer.c b/src/framebuffer.c
index a90bcaa..e3303b6 100644
--- a/src/framebuffer.c
+++ b/src/framebuffer.c
@@ -4,27 +4,38 @@
 
 #include <stdint.h>
 
-#define WIDTH  640
-#define HEIGHT 480
-#define DEPTH   32
+#define WIDTH     640
+#define HEIGHT    480
+#define DEPTH      32
+#define ALIGNMENT  16 // Framebuffer byte alignment.
 
 bool framebuffer_init(uint32_t* error) {
-  volatile const Mail* mail;
-  
-  volatile __attribute__((aligned(MAIL_ALIGN))) uint32_t InitFramebuffer[] = {
-    80, // Size in bytes aligned to MAIL_ALIGN(16).
+  volatile __attribute__((aligned(MAIL_ALIGN))) uint32_t ConfigureScreen[20] = {
+    80, // Size in bytes, aligned to MAIL_ALIGN.
     MAILBOX_REQUEST,
     TAG_FRAMEBUFFER_SET_PHYSICAL_SCREEN_SIZE, 8, MAILBOX_REQUEST, WIDTH, HEIGHT,
     TAG_FRAMEBUFFER_SET_VIRTUAL_SCREEN_SIZE,  8, MAILBOX_REQUEST, WIDTH, HEIGHT,
     TAG_FRAMEBUFFER_SET_DEPTH,                4, MAILBOX_REQUEST, DEPTH,
     TAG_END,
-    0, 0, 0 // Pad.
+    0, 0, 0 // Padding.
+  };
+  mbox_write(PROPERTY_CHANNEL, ConfigureScreen);
+  const Mail* response = mbox_read(PROPERTY_CHANNEL);
+  if (response->code != MAILBOX_SUCCESS) {
+    goto end;
+  }
+  
+  volatile __attribute__((aligned(MAIL_ALIGN))) uint32_t InitFramebuffer[8] = {
+    32, // Size in bytes, aligned to MAIL_ALIGN.
+    MAILBOX_REQUEST,
+    TAG_FRAMEBUFFER_ALLOCATE, 8, MAILBOX_REQUEST, ALIGNMENT, 0,
+    TAG_END
   };
-
   mbox_write(PROPERTY_CHANNEL, InitFramebuffer);
-  while ((mail = mbox_read(PROPERTY_CHANNEL)) != (volatile Mail*)(InitFramebuffer));
+  response = mbox_read(PROPERTY_CHANNEL);
   
-  *error = mail->code;
-  return mail->code == MAILBOX_SUCCESS;
+end:
+  *error = response->code;
+  return response->code == MAILBOX_SUCCESS;
 }
 
diff --git a/src/kernel.c b/src/kernel.c
index ac803cc..151028d 100644
--- a/src/kernel.c
+++ b/src/kernel.c
@@ -19,11 +19,14 @@ void main() {
   
   mbox_init();
   uart_init(raspi);
+  uart_print("Initializing framebuffer\n");
   success = framebuffer_init(&error);
   if (!success) {
-    uart_print("Failed to initialize framebuffer\n");
+    uart_print("Failed to initialize framebuffer:\n");
     if (error == MAILBOX_DELIVERY_ERROR) {
       uart_print("MAILBOX_DELIVERY_ERROR\n");
+    } else if (error == MAILBOX_ERROR) {
+      uart_print("MAILBOX_ERROR\n");
     }
     goto end;
   }
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) {
diff --git a/src/mailbox.h b/src/mailbox.h
index 3cca366..b35c7a6 100644
--- a/src/mailbox.h
+++ b/src/mailbox.h
@@ -55,6 +55,7 @@ typedef struct __attribute__((aligned(MAIL_ALIGN))) Mail {
   Tag      tags[];  // Variable quantity.
 } Mail;
 
+// TODO: Remove? Unused.
 #define MAIL_SIZE(TYPE) (sizeof(TYPE) + (2 * sizeof(uint32_t)))
 
 void        mbox_init();