diff --git a/src/cache.c b/src/cache.c
index 349d337f..9826f648 100644
--- a/src/cache.c
+++ b/src/cache.c
@@ -132,7 +132,7 @@ void map_4k_page(int logical, int physical) {
 #endif
 }
 
-void enable_MMU_and_IDCaches(void)
+void enable_MMU_and_IDCaches(int cached_screen_area, int cached_screen_size)
 {
 
    log_debug("enable_MMU_and_IDCaches");
@@ -193,26 +193,7 @@ void enable_MMU_and_IDCaches(void)
    {
       PageTable[base] = base << 20 | 0x04C02 | (shareable << 16) | (bb << 12);
    }
-#if defined(USE_CACHED_SCREEN)
-   for (; base < ((SCREEN_START_LO + CACHED_SCREEN_OFFSET) >> 20); base++)    //0x1E000000 + x00C00000
-   {
-      PageTable[base] = base << 20 | 0x01C02;     //uncached part of screen ram
-   }
-   for (; base < ((SCREEN_START_LO + SCREEN_SIZE) >> 20); base++)             //0x1EC00000 + x00400000
-   {
-      PageTable[base] = base << 20 | 0x04C02 | (shareable << 16) | (bb << 12) | (aa << 2);  //cached part of screen ram
-   }
-
-   for (; base < ((SCREEN_START_HI + CACHED_SCREEN_OFFSET) >> 20); base++)     //< 0x3E000000 + x00C00000
-   {
-      PageTable[base] = base << 20 | 0x01C02;
-   }
-    for (; base < uncached_threshold; base++)             //0x3EC00000 + x00400000
-   {
-      PageTable[base] = base << 20 | 0x04C02 | (shareable << 16) | (bb << 12) | (aa << 2);  //cached part of screen ram
-   }
-#endif
-   for (; base < uncached_threshold; base++)      // < 0x3F000000
+   for (; base < uncached_threshold; base++)
    {
       PageTable[base] = base << 20 | 0x01C02;
    }
@@ -222,6 +203,15 @@ void enable_MMU_and_IDCaches(void)
       PageTable[base] = base << 20 | 0x10C16;
    }
 
+#if defined(USE_CACHED_SCREEN)
+   if (cached_screen_area != 0) {
+       for (base = (cached_screen_area >> 20); base < ((cached_screen_area + cached_screen_size) >> 20); base++)
+       {
+          PageTable[base] = base << 20 | 0x04C02 | (shareable << 16) | (bb << 12) | (aa << 2);  //cached part of screen ram
+       }
+   }
+#endif
+
    // suppress a warning as we really do want to copy from src address 0!
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wnonnull"
diff --git a/src/cache.h b/src/cache.h
index 267847f2..00ffe320 100644
--- a/src/cache.h
+++ b/src/cache.h
@@ -22,7 +22,7 @@
 
 void map_4k_page(int logical, int physical);
 
-void enable_MMU_and_IDCaches(void);
+void enable_MMU_and_IDCaches(int cached_screen_area, int cached_screen_size);
 
 void CleanDataCache (void);
 
diff --git a/src/defs.h b/src/defs.h
index ccd0a43b..ae58a302 100644
--- a/src/defs.h
+++ b/src/defs.h
@@ -113,10 +113,8 @@
 //do not leave USE_ARM_CAPTURE uncommented during a release build as all versions will be ARM
 //#define USE_ARM_CAPTURE                   //uncomment to select ARM capture build
 
-#define SCREEN_START_LO         0x1E000000   // start of screen area
-#define SCREEN_START_HI         0x3E000000   // start of screen area
-#define SCREEN_SIZE             0x01000000   // size of screen area
-#define CACHED_SCREEN_OFFSET    0x00C00000   // offset to cached screen area
+#define CACHED_SCREEN_OFFSET    0x00B00000   // offset to cached screen area
+#define CACHED_SCREEN_SIZE      0x00100000   // size of cached screen area
 
 #if defined(RPI2)
 #define HAS_MULTICORE                     // indicates multiple cores are available
@@ -144,6 +142,7 @@
 #define HAS_MULTICORE                     // indicates multiple cores are available
 #define USE_CACHED_SCREEN                 // caches the upper half of the screen area and uses it for mode7 deinterlace
 #define USE_ALT_M7DEINTERLACE_CODE        // uses re-ordered code for mode7 deinterlace
+#define MODE7_ALWAYS_ARM                  // always runs mode7 capture code on ARM  
 #endif
 
 //#define USE_MULTICORE                     //can be used to add code in an extra core
diff --git a/src/rgb_to_fb.S b/src/rgb_to_fb.S
index 628f265f..8358a3b5 100644
--- a/src/rgb_to_fb.S
+++ b/src/rgb_to_fb.S
@@ -2224,6 +2224,8 @@ gpu_bench:
 #ifdef USE_MULTICORE
         .ltorg
 run_core:
+        mov r0, 0
+        mov r1, 0
         bl     enable_MMU_and_IDCaches
         bl    _enable_unaligned_access
         bl    _init_cycle_counter
diff --git a/src/rgb_to_hdmi.c b/src/rgb_to_hdmi.c
index 518bd2d3..d15d3830 100644
--- a/src/rgb_to_hdmi.c
+++ b/src/rgb_to_hdmi.c
@@ -538,13 +538,12 @@ static int last_height = -1;
     }
 
     if ((mp = RPI_PropertyGet(TAG_ALLOCATE_BUFFER))) {
-      capinfo->fb = (unsigned char*)mp->data.buffer_32[0];
-      log_info("Framebuffer address: %8.8X", (unsigned int)capinfo->fb);
+      unsigned int framebuffer = (unsigned int)mp->data.buffer_32[0];
+      // On the Pi 2/3 the mailbox returns the address with bits 31..30 set, which is wrong
+      capinfo->fb = (unsigned char *)(framebuffer & 0x3fffffff);
+      log_info("Framebuffer address: %8.8X (%8.8X)", (unsigned int)capinfo->fb, framebuffer);
     }
 
-    // On the Pi 2/3 the mailbox returns the address with bits 31..30 set, which is wrong
-    capinfo->fb = (unsigned char *)(((unsigned int) capinfo->fb) & 0x3fffffff);
-    //log_info("Framebuffer address masked: %8.8X", (unsigned int)capinfo->fb);
     //Initialize the palette
     osd_update_palette();
 
@@ -3142,7 +3141,7 @@ void rgb_to_hdmi_main() {
    set_scaling(get_scaling(), 2);
    resolution_warning = 0;
    clear = BIT_CLEAR;
-   
+
    while (1) {
       log_info("-----------------------LOOP------------------------");
       if (profile != last_profile || last_saved_config_number != saved_config_number) {
@@ -3222,7 +3221,7 @@ log_info("d = %08X, %08X", xdisplay_list[0], xdisplay_list[1]);
            log_info("ARM: GPIO read = %dns, MBOX read = %dns, Triple MBOX read = %dns (%dns/word)", (int)((double) benchmarkRAM(3) * 1000 / cpuspeed / 100000 + 0.5), (int)((double) benchmarkRAM(4) * 1000 / cpuspeed / 100000 + 0.5), triple, triple / 3);
            log_info("GPU: GPIO read = %dns, MBOX write = %dns", (int)((double) benchmarkRAM(1) * 1000 / cpuspeed / 100000 + 0.5), (int)((double) benchmarkRAM(2) * 1000 / cpuspeed / 100000 + 0.5));
            log_info("RAM: Cached read = %dns, Uncached screen read = %dns", (int)((double) benchmarkRAM(0x2000000) * 1000 / cpuspeed / 100000 + 0.5), (int)((double) benchmarkRAM((int)capinfo->fb) * 1000 / cpuspeed / 100000 + 0.5));
-                     
+
            if (cpld_fail_state == CPLD_MANUAL) {
                 rgb_to_fb(capinfo, extra_flags() | BIT_PROBE); // dummy mode7 probe to setup parms from capinfo
                 osd_set(0, 0, "Release buttons for CPLD recovery menu");
@@ -3562,31 +3561,55 @@ int show_detected_status(int line) {
 
 void kernel_main(unsigned int r0, unsigned int r1, unsigned int atags)
 {
-   RPI_AuxMiniUartInit(115200, 8);
+    RPI_AuxMiniUartInit(115200, 8);
+    rpi_mailbox_property_t *mp;
+    unsigned int frame_buffer_start = 0;
+    RPI_PropertyInit();
+    RPI_PropertyAddTag(TAG_ALLOCATE_BUFFER, 0x02000000);
+    RPI_PropertyAddTag(TAG_SET_PHYSICAL_SIZE, 64, 64);
+    RPI_PropertyAddTag(TAG_SET_VIRTUAL_SIZE, 64, 64);
+    RPI_PropertyAddTag(TAG_SET_DEPTH, capinfo->bpp);
+    RPI_PropertyProcess();
+        // FIXME: A small delay (like the log) is neccessary here
+        // or the RPI_PropertyGet seems to return garbage
+    int k = 0;    
+    for (int j = 0; j < 100000; j++) {
+        k = k + j;
+    }
+    if ((mp = RPI_PropertyGet(TAG_ALLOCATE_BUFFER))) {
+      frame_buffer_start = (unsigned int)mp->data.buffer_32[0];
+    }
+    frame_buffer_start &= 0x3fffffff;
 
-   enable_MMU_and_IDCaches();
-   _enable_unaligned_access();
+    enable_MMU_and_IDCaches(frame_buffer_start + CACHED_SCREEN_OFFSET, CACHED_SCREEN_SIZE);
 
-   log_info("***********************RESET***********************");
-   log_info("RGB to HDMI booted");
+    _enable_unaligned_access();
 
-   init_hardware();
+    log_info("***********************RESET***********************");
+    log_info("RGB to HDMI booted");
+
+    if (frame_buffer_start != 0) {
+       log_info("Marked framebuffer from %08X to %08X as cached", frame_buffer_start + CACHED_SCREEN_OFFSET, frame_buffer_start + CACHED_SCREEN_OFFSET + CACHED_SCREEN_SIZE);
+    } else {
+       log_info("No framebuffer area marked as cached");
+    }
+    init_hardware();
 
 #ifdef HAS_MULTICORE
-   int i;
-   printf("main running on core %u\r\n", _get_core());
-   for (i = 0; i < 10000000; i++);
+    int i;
+    printf("main running on core %u\r\n", _get_core());
+    for (i = 0; i < 10000000; i++);
 #ifdef USE_MULTICORE
-   start_core(1, _init_core);
+    start_core(1, _init_core);
 #else
-   start_core(1, _spin_core);
+    start_core(1, _spin_core);
 #endif
-   for (i = 0; i < 10000000; i++);
-   start_core(2, _spin_core);
-   for (i = 0; i < 10000000; i++);
-   start_core(3, _spin_core);
-   for (i = 0; i < 10000000; i++);
+    for (i = 0; i < 10000000; i++);
+    start_core(2, _spin_core);
+    for (i = 0; i < 10000000; i++);
+    start_core(3, _spin_core);
+    for (i = 0; i < 10000000; i++);
 #endif
 
-   rgb_to_hdmi_main();
+    rgb_to_hdmi_main();
 }