1 files changed, 62 insertions, 8 deletions

diff --git a/src/vcpu.c b/src/vcpu.c
index 67dea13..1f36b68 100644
--- a/src/vcpu.c
+++ b/src/vcpu.c
@@ -5,6 +5,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <unistd.h>
 #include <time.h>
 
 // Current VCPU state
@@ -13,6 +14,9 @@ VCPU_State State;
 void VCPUInit(){
   State.PC=ADDR_ROM;
   State.S=0;
+  State.dtst_ticks=0;
+  State.screen_ticks=0;
+  State.keypress=-1;
   srand(time(NULL));
 }
 
@@ -204,11 +208,20 @@ void VCPUExecute(){
     
   case 0xE:
     if(State.NN==0x9E){ // Skip if keypress in VX
-      if(KeypadIsPressed(State.V[State.X]&0x0F)){
-        State.PC+=2;
+      if(State.keypress >= 0){
+        if(State.V[State.X]&0x0F == State.keypress&0xF){
+          State.PC+=2;
+        }
+        State.keypress=-1;
       }
     }else if(State.NN==0xA1){ // Skip if not keypress in VX
-      if(!KeypadIsPressed(State.V[State.X]&0x0F))
+      if(State.keypress >=0){
+        if(State.V[State.X]&0x0F != State.keypress&0xF){
+          State.PC+=2;
+        }
+        State.keypress=-1;
+      }
+      else
         State.PC+=2;
     }
     break;
@@ -220,13 +233,15 @@ void VCPUExecute(){
       break;
       
     case 0x0A:
-      int key=KeypadGetPressed();
-      if(key >= 0){
-        State.V[State.X]=key&0x0F;
+      if(State.keypress >=0){
+        State.V[State.X]=State.keypress&0xF;
+        if(State.V[State.X]&0x0F != State.keypress&0xF){
+          State.PC+=2;
+          State.keypress=-1;
+        }
       }
       else
-        State.PC-=2; // Go back to last instruction (loop until key is pressed)
-        
+        State.PC-=2; // Go back to last instruction (loop until key is pressed)       
       break;
       
     case 0x15: // Set timer
@@ -267,6 +282,45 @@ void VCPUExecute(){
   }
 }
 
+void VCPUTick(){
+  struct timespec start, stop;
+  double duration, delay;
+
+  // Run and benchmark CPU pipeline
+  clock_gettime(CLOCK_REALTIME, &start);
+  VCPUFetch();
+  VCPUDecode();
+  VCPUExecute();
+  State.dtst_ticks++;
+  State.screen_ticks++;
+  clock_gettime(CLOCK_REALTIME, &stop);
+
+  // Adjust pipeline duration
+  duration=(stop.tv_sec - start.tv_sec) + (stop.tv_nsec - start.tv_nsec)*1e-9;
+  delay=1.0/VCPU_FREQ-duration;
+  if(delay>0){
+    usleep(delay*1e6);
+  }
+
+  // Update Delay Timer and Sound Timer
+  if(State.dtst_ticks>=(1.0*VCPU_FREQ/DTST_FREQ)){
+    State.dtst_ticks=0;
+    if(State.DT>0)
+      State.DT--;
+  }
+
+  // Refresh screen
+  if(State.screen_ticks>=(1.0*VCPU_FREQ/SCREEN_FREQ)){
+    State.screen_ticks=0;
+    ScreenUpdate();
+  }
+
+  // Update keypressed
+  int keypress=KeypadGetPressed();
+  if(keypress>=0)
+    State.keypress=keypress;
+}
+
 void VCPUDump(){
   printf("opcode: 0x%04x\n",State.opcode&0xFFFF);
   printf("X: 0x%01x\n",State.X);