Kernel: Flush data cache before passing a buffer to the VC Mailbox

Otherwise, the message's contents might be in the cache only, so
VideoCore will read stale/garbage data from main memory.

This fixes framebuffer setup on bare metal with the data cache enabled.

Kernel/Arch/aarch64/ASM_wrapper.h

@@ -125,6 +125,25 @@ inline u64 read_rndrrs()
     return value;
 }
 
+inline FlatPtr get_cache_line_size()
+{
+    FlatPtr ctr_el0;
+    asm volatile("mrs %[value], ctr_el0"
+                 : [value] "=r"(ctr_el0));
+    auto log2_size = (ctr_el0 >> 16) & 0xF;
+    return 1 << log2_size;
+}
+
+inline void flush_data_cache(FlatPtr start, size_t size)
+{
+    auto const cache_size = get_cache_line_size();
+    for (FlatPtr addr = align_down_to(start, cache_size); addr < start + size; addr += cache_size)
+        asm volatile("dc civac, %[addr]" ::[addr] "r"(addr)
+                     : "memory");
+    asm volatile("dsb sy" ::
+                     : "memory");
+}
+
 }
 
 namespace Kernel {

Kernel/Arch/aarch64/RPi/Mailbox.cpp

@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: BSD-2-Clause
  */
 
+#include <Kernel/Arch/aarch64/ASM_wrapper.h>
 #include <Kernel/Arch/aarch64/RPi/MMIO.h>
 #include <Kernel/Arch/aarch64/RPi/Mailbox.h>
 
@@ -90,6 +91,10 @@ bool Mailbox::send_queue(void* queue, u32 queue_size) const
 
     // The mailbox message is 32-bit based, so this assumes that message is in the first 4 GiB.
     u32 request = static_cast<u32>(reinterpret_cast<FlatPtr>(queue) & ~0xF) | (channel & 0xF);
+
+    // The queue buffer might point to normal cached memory, so flush any writes that are in cache and not visible to VideoCore.
+    Aarch64::Asm::flush_data_cache((FlatPtr)queue, queue_size);
+
     mmio.write(MBOX_WRITE_DATA, request);
 
     for (;;) {