diff --git a/src/defs.h b/src/defs.h
index 00816630..5050ed0c 100644
--- a/src/defs.h
+++ b/src/defs.h
@@ -32,6 +32,9 @@
 #define BIT_CALIBRATE    0x20
 #define BIT_INITIALIZE   0x40
 
+// R0 return value bits
+#define BIT_CAL          0x02
+
 #define BIT_BOTH_BUFFERS (BIT_DRAW_BUFFER | BIT_DISP_BUFFER)
 
 // Define the size of the Pi Framebuffer
@@ -100,6 +103,7 @@
 // Quad Pixel input on GPIOs 2..13
 #define PIXEL_BASE   (2)
 
+#define CAL_PIN      (16)
 #define PSYNC_PIN    (17)
 #define CSYNC_PIN    (18)
 #define MODE7_PIN    (19)
@@ -109,6 +113,7 @@
 
 #define LED_PIN      (47)
 
+#define CAL_MASK      (1 << CAL_PIN)
 #define PSYNC_MASK    (1 << PSYNC_PIN)
 #define CSYNC_MASK    (1 << CSYNC_PIN)
 #define MODE7_MASK    (1 << MODE7_PIN)
diff --git a/src/rgb_to_fb.S b/src/rgb_to_fb.S
index 29a15dc0..a7728c11 100644
--- a/src/rgb_to_fb.S
+++ b/src/rgb_to_fb.S
@@ -175,6 +175,9 @@ frame:
         movge  r0, #1         // Mode 7
         tst    r3, #BIT_PROBE
         bne    exit
+        tst    r8, #CAL_MASK  // Is the cal button pressed (active low)
+        orreq  r0, #BIT_CAL   //
+        beq    exit
         tst    r3, #BIT_MODE7
         moveq  r5, #0         // Modes 0-6
         movne  r5, #1         // Mode 7
diff --git a/src/rgb_to_hdmi.c b/src/rgb_to_hdmi.c
index 9f349c8f..0e4c5cc3 100644
--- a/src/rgb_to_hdmi.c
+++ b/src/rgb_to_hdmi.c
@@ -463,6 +463,8 @@ void calibrate_sampling(int mode7, int chars_per_line) {
 
 void rgb_to_hdmi_main() {
    int mode7;
+   int last_mode7;
+   int result;
 
    // The divisor us now the same for both modes
    log_debug("Setting up divisor");
@@ -481,14 +483,23 @@ void rgb_to_hdmi_main() {
       log_debug("Done setting up frame buffer");
 
       int chars_per_line = mode7 ? MODE7_CHARS_PER_LINE : DEFAULT_CHARS_PER_LINE;
+      
+      do {
 
-      for (int c = 0; c < NUM_CAL_PASSES; c++) {
-         calibrate_sampling(mode7, chars_per_line);
-      }
+         log_debug("Entering rgb_to_fb");
+         result = rgb_to_fb(fb, chars_per_line, pitch, mode7);
+         log_debug("Leaving rgb_to_fb, result= %d", result);
 
-      log_debug("Entering rgb_to_fb %d", mode7);
-      mode7 = rgb_to_fb(fb, chars_per_line, pitch, mode7);
-      log_debug("Leaving rgb_to_fb %d", mode7);
+         if (result & BIT_CAL) {
+            for (int c = 0; c < NUM_CAL_PASSES; c++) {
+               calibrate_sampling(mode7, chars_per_line);
+            }
+         }
+
+         last_mode7 = mode7;
+         mode7 = result & BIT_MODE7;
+
+      } while (mode7 == last_mode7);
    }
 }
 
diff --git a/vhdl/RGBtoHDMI.ucf b/vhdl/RGBtoHDMI.ucf
index 9c9ff00b..7ff44cfc 100644
--- a/vhdl/RGBtoHDMI.ucf
+++ b/vhdl/RGBtoHDMI.ucf
@@ -16,8 +16,8 @@ NET "SW"        LOC = "P18";       # input
 NET "mode7"     LOC = "P19";       # input
 NET "sp_clk"    LOC = "P1";        # input
 NET "sp_data"   LOC = "P2";        # input
-NET  "SWout"    LOC = "P3";        # output
-NET  "test"     LOC = "P40";       # output
+NET "SWout"     LOC = "P3";        # output
+NET "test"      LOC = "P40";       # output
 
 NET "quad(0)"   LOC = "P37";       # output
 NET "quad(1)"   LOC = "P36";       # output