diff --git a/src/videocore.s b/src/videocore.s
index 1ccb534b..1763b4e6 100644
--- a/src/videocore.s
+++ b/src/videocore.s
@@ -46,8 +46,8 @@ wait_psync_lo\@:
    btst   r0, MUX_BIT
    and    r0, r6
    bsetne r0, ALT_MUX_BIT  #move mux bit to position in 16 bit sample
-   or     r0, r2           #merge bit state
    sub    r3, 1
+   or     r0, r2           #merge bit state
 .endm
 
 .macro HI_PSYNC_CAPTURE
@@ -59,8 +59,8 @@ wait_psync_hi\@:
    and    r1, r6
    bsetne r1, ALT_MUX_BIT  #move mux bit to position in 16 bit sample
    lsl    r1, 16           #merge lo and hi samples
-   or     r0, r1
    cmp    r3, 0
+   or     r0, r1
 .endm
 
 .macro EDGE_DETECT
@@ -107,38 +107,36 @@ not_mbox_write_benchmark:
    mov    r6, VIDEO_MASK
    mov    r7, COMMAND_MASK
    mov    r8, DEFAULT_BIT_STATE
-   mov    r2, 0
-   st     r2, DATA_BUFFER_0_offset(r5)
-   st     r2, DATA_BUFFER_1_offset(r5)
-   st     r2, DATA_BUFFER_2_offset(r5)
-   st     r2, DATA_BUFFER_3_offset(r5)
-   st     r2, DATA_BUFFER_4_offset(r5)
-   st     r2, DATA_BUFFER_5_offset(r5)
-   st     r2, GPU_SYNC_offset(r5)
+   mov    r12, 0                       # remains at zero for rest of the code
+   st     r12, DATA_BUFFER_0_offset(r5)
+   st     r12, DATA_BUFFER_1_offset(r5)
+   st     r12, DATA_BUFFER_2_offset(r5)
+   st     r12, DATA_BUFFER_3_offset(r5)
+   st     r12, DATA_BUFFER_4_offset(r5)
+   st     r12, DATA_BUFFER_5_offset(r5)
 
 wait_for_command:
-   ld     r2, DATA_BUFFER_0_offset(r5)
-   bset   r2, FINAL_BIT
-   st     r2, DATA_BUFFER_0_offset(r5)
-   ld     r2, DATA_BUFFER_1_offset(r5)
-   bset   r2, FINAL_BIT
-   st     r2, DATA_BUFFER_1_offset(r5)
+   ld     r0, DATA_BUFFER_0_offset(r5)
+   ld     r1, DATA_BUFFER_1_offset(r5)
    ld     r2, DATA_BUFFER_2_offset(r5)
+   ld     r3, DATA_BUFFER_3_offset(r5)
+   ld     r9, DATA_BUFFER_4_offset(r5)
+   ld     r10, DATA_BUFFER_5_offset(r5)      
+   st     r12, GPU_COMMAND_offset(r5)    #set command register to 0
+   st     r12, GPU_SYNC_offset(r5)       #set sync register to 0
+   bset   r0, FINAL_BIT
+   bset   r1, FINAL_BIT
    bset   r2, FINAL_BIT
-   st     r2, DATA_BUFFER_2_offset(r5)
-   ld     r2, DATA_BUFFER_3_offset(r5)
-   bset   r2, FINAL_BIT
-   st     r2, DATA_BUFFER_3_offset(r5)
-   ld     r2, DATA_BUFFER_4_offset(r5)
-   bset   r2, FINAL_BIT
-   st     r2, DATA_BUFFER_4_offset(r5)
-   ld     r2, DATA_BUFFER_5_offset(r5)
-   bset   r2, FINAL_BIT
-   st     r2, DATA_BUFFER_5_offset(r5)
+   bset   r3, FINAL_BIT
+   bset   r9, FINAL_BIT
+   bset   r10, FINAL_BIT
 
-   mov    r2, 0
-   st     r2, GPU_COMMAND_offset(r5)    #set command register to 0
-   st     r2, GPU_SYNC_offset(r5)       #set sync register to 0
+   st     r0, DATA_BUFFER_0_offset(r5)
+   st     r1, DATA_BUFFER_1_offset(r5)
+   st     r2, DATA_BUFFER_2_offset(r5)
+   st     r3, DATA_BUFFER_3_offset(r5)
+   st     r9, DATA_BUFFER_4_offset(r5)
+   st     r10, DATA_BUFFER_5_offset(r5)
 
    mov    r2, r8                        #set the default state of the control bits