pio: run in sync with CPU core, honor ClockDiv

Author: c1570

src/peripherals/pio.ts

@@ -138,6 +138,9 @@ export class StateMachine {
 
   clockDivInt: number = 1;
   clockDivFrac: number = 0;
+  curClockInt: number = 0;
+  curClockFrac: number = 0;
+  remainingDelay: number = 0;
   execCtrl = 0x1f << 12;
   shiftCtrl = 0b11 << 18;
   pinCtrl = 0x5 << 26;
@@ -436,7 +439,7 @@ export class StateMachine {
     }
   }
 
-  executeInstruction(opcode: number) {
+  executeInstruction(opcode: number): number {
     const arg = opcode & 0xff;
     switch (opcode >>> 13) {
       /* JMP */
@@ -654,14 +657,16 @@ export class StateMachine {
 
     if (this.execValid) {
       this.execValid = false;
-      this.executeInstruction(this.execOpcode);
+      return this.executeInstruction(this.execOpcode);
     } else if (this.waiting) {
       if (this.waitDelay < 0) {
         this.waitDelay = delay;
       }
       this.checkWait();
+      return delay;
     } else {
       this.cycles += delay;
+      return delay;
     }
   }
 
@@ -683,6 +688,27 @@ export class StateMachine {
   }
 
   step() {
+    if (!this.enabled) {
+      return;
+    }
+
+    this.curClockFrac += this.clockDivFrac;
+    if (this.curClockFrac > 0xff) {
+      this.curClockInt++;
+      this.curClockFrac -= 0x100;
+    }
+    this.curClockInt++;
+    if (this.curClockInt < this.clockDivInt) {
+      return;
+    } else {
+      this.curClockInt -= this.clockDivInt;
+    }
+
+    if (this.remainingDelay > 0) {
+      this.remainingDelay--;
+      return;
+    }
+
     if (this.waiting) {
       this.checkWait();
       if (this.waiting) {
@@ -691,7 +717,7 @@ export class StateMachine {
     }
 
     this.updatePC = true;
-    this.executeInstruction(this.pio.instructions[this.pc]);
+    this.remainingDelay += this.executeInstruction(this.pio.instructions[this.pc]);
     if (this.updatePC) {
       this.nextPC();
     }
@@ -833,6 +859,9 @@ export class StateMachine {
 
   restart() {
     this.cycles = 0;
+    this.curClockInt = 0;
+    this.curClockFrac = 0;
+    this.remainingDelay = 0;
     this.inputShiftCount = 0;
     this.outputShiftCount = 32;
     this.inputShiftReg = 0;
@@ -1080,7 +1109,6 @@ export class RPPIO extends BasePeripheral implements Peripheral {
         const shouldRun = value & 0xf;
         if (this.stopped && shouldRun) {
           this.stopped = false;
-          this.run();
         }
         if (!shouldRun) {
           this.stopped = true;
@@ -1179,21 +1207,15 @@ export class RPPIO extends BasePeripheral implements Peripheral {
   }
 
   step() {
+    if (this.stopped) {
+      return;
+    }
     for (const machine of this.machines) {
       machine.step();
     }
     this.checkChangedPins();
   }
 
-  run() {
-    for (let i = 0; i < 1000 && !this.stopped; i++) {
-      this.step();
-    }
-    if (!this.stopped) {
-      this.runTimer = setTimeout(() => this.run(), 0);
-    }
-  }
-
   stop() {
     for (const machine of this.machines) {
       machine.enabled = false;

src/rp2040.ts

@@ -349,15 +349,25 @@ export class RP2040 {
   }
 
   step() {
+    let coreStartCycles = this.core.cycles;
     this.core.executeInstruction();
+    for(let cycle = coreStartCycles; cycle < this.core.cycles; cycle++) {
+      this.pio[0].step();
+      this.pio[1].step();
+    }
   }
 
   execute() {
     this.clock.resume();
     this.executeTimer = null;
     this.stopped = false;
     for (let i = 0; i < 100000 && !this.stopped && !this.core.waiting; i++) {
+      let coreStartCycles = this.core.cycles;
       this.core.executeInstruction();
+      for(let cycle = coreStartCycles; cycle < this.core.cycles; cycle++) {
+        this.pio[0].step();
+        this.pio[1].step();
+      }
     }
     if (!this.stopped) {
       this.executeTimer = setTimeout(() => this.execute(), 0);