Index: c/src/lib/libbsp/arm/raspberrypi/console/fb.c
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/console/fb.c	(revision 842005e432136953d670e39cedc2d665ea949e7b)
+++ c/src/lib/libbsp/arm/raspberrypi/console/fb.c	(revision 3d3ad4dc1c54306aea643e66c377a7d06893f07b)
@@ -163,9 +163,13 @@
 static int find_mode_from_vc( void )
 {
+  int res;
+  unsigned int width;
+  unsigned int height;
   bcm2835_get_display_size_entries entries;
 
-  bcm2835_mailbox_get_display_size( &entries );
-  unsigned int width = entries.width;
-  unsigned int height = entries.height;
+  res = bcm2835_mailbox_get_display_size( &entries );
+
+  width = entries.width;
+  height = entries.height;
 
   if ( width == 0 || height == 0 ) {
@@ -176,6 +180,7 @@
     fb_var_info.yres = height;
   }
-
-  return 0;
+  printk("find_mode_from_vc %u x %u, res %d\n", width, height, res);
+
+  return res;
 }
 
@@ -200,4 +205,5 @@
 int rpi_fb_init( void )
 {
+  int res;
   int                               mode_from_cmdline;
   bcm2835_init_frame_buffer_entries init_frame_buffer_entries;
@@ -244,5 +250,14 @@
   init_frame_buffer_entries.overscan_top = 0;
   init_frame_buffer_entries.overscan_bottom = 0;
-  bcm2835_mailbox_init_frame_buffer( &init_frame_buffer_entries );
+  printk("bcm2835_mailbox_init_frame_buffer ...\n");
+  res = bcm2835_mailbox_init_frame_buffer( &init_frame_buffer_entries );
+  printk("bcm2835_mailbox_init_frame_buffer returned %d\n", res);
+  if (res != 0) {
+    printk("bcm2835_mailbox_init_frame_buffer retry ...\n");
+    res = bcm2835_mailbox_init_frame_buffer( &init_frame_buffer_entries );
+    printk("bcm2835_mailbox_init_frame_buffer returned %d\n", res);
+    if (res != 0)
+      return RPI_FB_INIT_SETUP_FAILED;
+  }
 
   bcm2835_get_pitch_entries get_pitch_entries;
@@ -258,4 +273,6 @@
   if ( fb_fix_info.smem_start == NULL )
     return RPI_FB_INIT_START_ADDR_UNKNOWN;
+
+  printk("fb_fix_info.smem_start %p\n", fb_fix_info.smem_start);
 
   arm_cp15_set_translation_table_entries( (void *) fb_fix_info.smem_start,
Index: c/src/lib/libbsp/arm/raspberrypi/include/rpi-fb.h
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/include/rpi-fb.h	(revision 842005e432136953d670e39cedc2d665ea949e7b)
+++ c/src/lib/libbsp/arm/raspberrypi/include/rpi-fb.h	(revision 3d3ad4dc1c54306aea643e66c377a7d06893f07b)
@@ -42,5 +42,6 @@
   RPI_FB_INIT_CMDLINE_NO_MODE_REQ = -5,
   RPI_FB_INIT_MODE_PROBE_ERROR = -6,
-  RPI_FB_INIT_START_ADDR_UNKNOWN = -7,
+  RPI_FB_INIT_SETUP_FAILED = -7,
+  RPI_FB_INIT_START_ADDR_UNKNOWN = -8,
 };
 
Index: c/src/lib/libbsp/arm/raspberrypi/misc/vc.c
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/misc/vc.c	(revision 842005e432136953d670e39cedc2d665ea949e7b)
+++ c/src/lib/libbsp/arm/raspberrypi/misc/vc.c	(revision 3d3ad4dc1c54306aea643e66c377a7d06893f07b)
@@ -32,4 +32,6 @@
 #endif
 
+#define BCM2835_VC_MEMORY_MAPPING_MASK 0x3fffffff
+
 static inline bool bcm2835_mailbox_buffer_suceeded(
   const bcm2835_mbox_buf_hdr *hdr )
@@ -41,7 +43,9 @@
 static inline int bcm2835_mailbox_send_read_buffer( void *buf )
 {
+  RTEMS_COMPILER_MEMORY_BARRIER();
   raspberrypi_mailbox_write( BCM2835_MBOX_CHANNEL_PROP_AVC,
     (unsigned int) buf + BCM2835_VC_MEMORY_MAPPING );
   raspberrypi_mailbox_read( BCM2835_MBOX_CHANNEL_PROP_AVC );
+  RTEMS_COMPILER_MEMORY_BARRIER();
   return 0;
 }
@@ -67,13 +71,14 @@
   bcm2835_get_display_size_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_display_size get_display_size;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   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 ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -86,4 +91,7 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_display_size ) )
+    return -3;
+
   return 0;
 }
@@ -92,5 +100,5 @@
   bcm2835_init_frame_buffer_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_display_size set_display_size;
@@ -104,5 +112,6 @@
     bcm2835_mbox_tag_get_pitch get_pitch;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   BCM2835_MBOX_INIT_BUF( &buffer );
   BCM2835_MBOX_INIT_TAG( &buffer.set_display_size,
@@ -138,5 +147,5 @@
   BCM2835_MBOX_INIT_TAG_NO_REQ( &buffer.get_pitch,
     BCM2835_MAILBOX_TAG_GET_PITCH );
-  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( &buffer ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -148,10 +157,6 @@
   _entries->yvirt = buffer.set_virtual_size.body.resp.vheight;
   _entries->depth = buffer.set_depth.body.resp.depth;
-#if ( BSP_IS_RPI2 == 1 )
   _entries->base = buffer.allocate_buffer.body.resp.base;
-#else
-  _entries->base = buffer.allocate_buffer.body.resp.base -
-                   BCM2835_VC_MEMORY_MAPPING;
-#endif
+  _entries->base &= BCM2835_VC_MEMORY_MAPPING_MASK;
   _entries->size = buffer.allocate_buffer.body.resp.size;
   _entries->pixel_order = buffer.set_pixel_order.body.resp.pixel_order;
@@ -167,4 +172,10 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.allocate_buffer ) )
+    return -3;
+
+  if ( _entries->base < 0x100000 )
+    return -4;
+
   return 0;
 }
@@ -172,13 +183,14 @@
 int bcm2835_mailbox_get_pitch( bcm2835_get_pitch_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_pitch get_pitch;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   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 ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -190,4 +202,7 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_pitch ) )
+    return -3;
+
   return 0;
 }
@@ -197,13 +212,14 @@
   int i;
 
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_cmd_line get_cmd_line;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   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 ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -221,4 +237,7 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_cmd_line ) )
+    return -3;
+
   return 0;
 }
@@ -226,9 +245,10 @@
 int bcm2835_mailbox_set_power_state( bcm2835_set_power_state_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_power_state set_power_state;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   BCM2835_MBOX_INIT_BUF( &buffer );
   BCM2835_MBOX_INIT_TAG( &buffer.set_power_state,
@@ -236,5 +256,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 ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -252,13 +272,14 @@
 int bcm2835_mailbox_get_arm_memory( bcm2835_get_arm_memory_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_arm_memory get_arm_memory;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   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 ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -271,4 +292,7 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_arm_memory ) )
+    return -3;
+
   return 0;
 }
@@ -276,13 +300,14 @@
 int bcm2835_mailbox_get_vc_memory( bcm2835_get_vc_memory_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_vc_memory get_vc_memory;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   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 ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -295,4 +320,7 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_vc_memory ) )
+    return -3;
+
   return 0;
 }
@@ -301,13 +329,14 @@
   bcm2835_mailbox_get_fw_rev_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_fw_rev get_fw_rev;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   BCM2835_MBOX_INIT_BUF( &buffer );
   BCM2835_MBOX_INIT_TAG_NO_REQ( &buffer.get_fw_rev,
     BCM2835_MAILBOX_TAG_FIRMWARE_REVISION );
-  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( &buffer ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -319,4 +348,7 @@
     return -2;
 
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_fw_rev ) )
+    return -3;
+
   return 0;
 }
@@ -324,13 +356,14 @@
 int bcm2835_mailbox_get_board_model( bcm2835_get_board_spec_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_board_spec get_board_model;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   BCM2835_MBOX_INIT_BUF( &buffer );
   BCM2835_MBOX_INIT_TAG_NO_REQ( &buffer.get_board_model,
     BCM2835_MAILBOX_TAG_GET_BOARD_MODEL );
-  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( &buffer ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -341,4 +374,7 @@
   if ( !bcm2835_mailbox_buffer_suceeded( &buffer.hdr ) )
     return -2;
+
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_board_model ) )
+    return -3;
 
   return 0;
@@ -348,13 +384,14 @@
   bcm2835_get_board_spec_entries *_entries )
 {
-  struct {
+  struct BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE {
     bcm2835_mbox_buf_hdr hdr;
     bcm2835_mbox_tag_get_board_spec get_board_revision;
     uint32_t end_tag;
-  } buffer BCM2835_MBOX_BUF_ALIGN_ATTRIBUTE;
+    uint32_t padding_reserve[16];
+  } buffer;
   BCM2835_MBOX_INIT_BUF( &buffer );
   BCM2835_MBOX_INIT_TAG_NO_REQ( &buffer.get_board_revision,
     BCM2835_MAILBOX_TAG_GET_BOARD_VERSION );
-  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( &buffer ) );
+  bcm2835_mailbox_buffer_flush_and_invalidate( &buffer, sizeof( buffer ) );
 
   if ( bcm2835_mailbox_send_read_buffer( &buffer ) )
@@ -366,4 +403,7 @@
     return -2;
 
-  return 0;
-}
+  if ( !BCM2835_MBOX_TAG_REPLY_IS_SET( &buffer.get_board_revision ) )
+    return -3;
+
+  return 0;
+}
Index: c/src/lib/libbsp/arm/raspberrypi/misc/vc_defines.h
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/misc/vc_defines.h	(revision 842005e432136953d670e39cedc2d665ea949e7b)
+++ c/src/lib/libbsp/arm/raspberrypi/misc/vc_defines.h	(revision 3d3ad4dc1c54306aea643e66c377a7d06893f07b)
@@ -95,7 +95,10 @@
 } bcm2835_mbox_tag_hdr;
 
+#define BCM2835_MBOX_TAG_REPLY_IS_SET( _t_ ) \
+   ( ( _t_ )->tag_hdr.val_len & 0x80000000 )
+
 #define BCM2835_MBOX_INIT_BUF( _m_ ) { \
-    memset( ( _m_ ), 0, sizeof( *( _m_ ) ) ); \
-    ( _m_ )->hdr.buf_size = sizeof( *( _m_ ) ); \
+    memset( ( _m_ ), 0,  sizeof( *( _m_ ) ) ); \
+    ( _m_ )->hdr.buf_size = (void *)&(( _m_ )->end_tag) + 4 - (void *)( _m_ ); \
     ( _m_ )->hdr.buf_code = BCM2835_MBOX_BUF_CODE_PROCESS_REQUEST; \
     ( _m_ )->end_tag = 0; \
Index: c/src/lib/libbsp/arm/raspberrypi/startup/bspgetworkarea.c
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/startup/bspgetworkarea.c	(revision 842005e432136953d670e39cedc2d665ea949e7b)
+++ c/src/lib/libbsp/arm/raspberrypi/startup/bspgetworkarea.c	(revision 3d3ad4dc1c54306aea643e66c377a7d06893f07b)
@@ -71,7 +71,8 @@
 
   memset( &vc_entry, 0, sizeof(vc_entry) );
-  bcm2835_mailbox_get_vc_memory( &vc_entry );
-  if (vc_entry.base != 0)
-    ram_end = ram_end > vc_entry.base? vc_entry.base: ram_end;
+  if (bcm2835_mailbox_get_vc_memory( &vc_entry ) >= 0) {
+    if (vc_entry.base > 10 * 1024 *1024)
+      ram_end = ram_end > vc_entry.base? vc_entry.base: ram_end;
+  }
   bsp_work_area_initialize_default( (void *) work_base, ram_end - work_base );
 }
Index: c/src/lib/libbsp/arm/raspberrypi/startup/cmdline.c
===================================================================
--- c/src/lib/libbsp/arm/raspberrypi/startup/cmdline.c	(revision 842005e432136953d670e39cedc2d665ea949e7b)
+++ c/src/lib/libbsp/arm/raspberrypi/startup/cmdline.c	(revision 3d3ad4dc1c54306aea643e66c377a7d06893f07b)
@@ -27,5 +27,6 @@
 {
   memset(&rpi_cmdline_entries, 0, sizeof(rpi_cmdline_entries));
-  bcm2835_mailbox_get_cmdline(&rpi_cmdline_entries);
+  if (bcm2835_mailbox_get_cmdline(&rpi_cmdline_entries) < 0)
+     return NULL;
   return rpi_cmdline_entries.cmdline;
 }
@@ -35,5 +36,6 @@
   if (rpi_cmdline_ready <= 0) {
     const char *line = rpi_cmdline_get_raw();
-    strncpy(rpi_cmdline_cached, line, MAX_CMDLINE_LENGTH - 1);
+    if (line != NULL)
+      strncpy(rpi_cmdline_cached, line, MAX_CMDLINE_LENGTH - 1);
     rpi_cmdline_cached[MAX_CMDLINE_LENGTH - 1] = 0;
     rpi_cmdline_ready = 1;
