Index: c/src/lib/libbsp/arm/raspberrypi/misc/vc.c
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/misc/vc.c	(revision 3e2a3c49480b9888362d016dd202edd562d9e069)
+++ c/src/lib/libbsp/arm/raspberrypi/misc/vc.c	(revision fa239ff26c5bafa3ffeecfaa130084e7cacafa12)
@@ -23,4 +23,5 @@
 #include <bsp/mailbox.h>
 #include <bsp/vc.h>
+#include <libcpu/arm-cp15.h>
 #include "vc_defines.h"
 
@@ -46,4 +47,24 @@
 }
 
+/*
+ * When cache is enabled then content of buffer exchanged
+ * with VideoCore has to be propagated through ARM11/Cortex-A7
+ * caches
+ */
+static inline void
+bcm2835_mailbox_buffer_flush_and_invalidate(void *buf, size_t size)
+{
+  uint32_t sctlr_val;
+
+  sctlr_val = arm_cp15_get_control();
+
+  arm_cp15_drain_write_buffer();
+  if (sctlr_val & (ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
+    arm_cp15_drain_write_buffer();
+    rtems_cache_flush_multiple_data_lines(buf, size);
+    rtems_cache_invalidate_multiple_data_lines(buf, size);
+  }
+}
+
 #define BCM2835_MBOX_VAL_LENGTH_MASK(_val_len) \
   (_val_len&(~BCM2835_MBOX_TAG_VAL_LEN_RESPONSE))
@@ -56,8 +77,9 @@
     bcm2835_mbox_tag_display_size get_display_size;
     uint32_t end_tag;
-  }buffer __attribute__((aligned (16)));
+  }buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG_NO_REQ(&buffer.get_display_size,
     BCM2835_MAILBOX_TAG_GET_DISPLAY_SIZE);
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
@@ -84,5 +106,5 @@
     bcm2835_mbox_tag_get_pitch get_pitch;
     uint32_t end_tag;
-  } buffer __attribute__((aligned (16)));
+  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG(&buffer.set_display_size,
@@ -118,4 +140,5 @@
   BCM2835_MBOX_INIT_TAG_NO_REQ(&buffer.get_pitch,
     BCM2835_MAILBOX_TAG_GET_PITCH);
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
@@ -152,8 +175,9 @@
     bcm2835_mbox_tag_get_pitch get_pitch;
     uint32_t end_tag;
-  }buffer __attribute__((aligned (16)));
+  }buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG_NO_REQ(&buffer.get_pitch,
     BCM2835_MAILBOX_TAG_GET_PITCH);
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
@@ -172,8 +196,9 @@
     bcm2835_mbox_tag_get_cmd_line get_cmd_line;
     uint32_t end_tag;
-  }buffer __attribute__((aligned (16)));
+  }buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG_NO_REQ(&buffer.get_cmd_line,
     BCM2835_MAILBOX_TAG_GET_CMD_LINE);
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
@@ -197,5 +222,5 @@
     bcm2835_mbox_tag_power_state set_power_state;
     uint32_t end_tag;
-  }buffer __attribute__((aligned (16)));
+  }buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG(&buffer.set_power_state,
@@ -203,4 +228,5 @@
   buffer.set_power_state.body.req.dev_id = _entries->dev_id;
   buffer.set_power_state.body.req.state = _entries->state;
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
@@ -219,8 +245,9 @@
     bcm2835_mbox_tag_get_arm_memory get_arm_memory;
     uint32_t end_tag;
-  }buffer __attribute__((aligned (16)));
+  }buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG_NO_REQ(&buffer.get_arm_memory,
     BCM2835_MAILBOX_TAG_GET_ARM_MEMORY);
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
@@ -239,8 +266,9 @@
     bcm2835_mbox_tag_get_vc_memory get_vc_memory;
     uint32_t end_tag;
-  }buffer __attribute__((aligned (16)));
+  }buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
   BCM2835_MBOX_INIT_BUF(&buffer);
   BCM2835_MBOX_INIT_TAG_NO_REQ(&buffer.get_vc_memory,
     BCM2835_MAILBOX_TAG_GET_VC_MEMORY);
+  bcm2835_mailbox_buffer_flush_and_invalidate(&buffer, sizeof(&buffer));
   if (bcm2835_mailbox_send_read_buffer(&buffer))
     return -1;
Index: c/src/lib/libbsp/arm/raspberrypi/misc/vc_defines.h
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/misc/vc_defines.h	(revision 3e2a3c49480b9888362d016dd202edd562d9e069)
+++ c/src/lib/libbsp/arm/raspberrypi/misc/vc_defines.h	(revision fa239ff26c5bafa3ffeecfaa130084e7cacafa12)
@@ -115,4 +115,22 @@
     (_t_)->tag_hdr.val_len = 0; \
   }
+
+/*
+ * Mailbox buffers has to be aligned to 16 bytes because
+ * 4 LSB bits of the BCM2835_MBOX_WRITE and BCM2835_MBOX_READ
+ * registers are used to pass channel number.
+ *
+ * But there is another requirement for buffer allocation
+ * as well when interface is called after cache is enabled.
+ * The buffer should not share cache line with another variable
+ * which can be updated during data exchange with VideoCore.
+ * If cache is filled to satisfy another variable update
+ * during VideoCore output is stored into main memory then
+ * part of received data can be lost.
+ *
+ * Cache line length is 64 bytes for RPi2 Cortex-A7 data cache
+ * so align buffers to this value.
+ */
+#define BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE __attribute__((aligned (64)))
 
 /* Video Core */
@@ -245,4 +263,7 @@
 #define BCM2835_MAILBOX_TAG_SET_TURBO           0x00038009
 
+#define BCM2835_MAILBOX_TAG_GET_DOMAIN_STATE    0x00030030
+#define BCM2835_MAILBOX_TAG_SET_DOMAIN_STATE    0x00038030
+
 /* Voltage */
 #define BCM2835_MAILBOX_VOLTAGE_RESERVED_UVID        0x000000000
