feat(rp2040): implement SVC / SVCall

wokwi · Apr 27, 2021 · 383177e · 383177e
1 parent 5ebab17
commit 383177e
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 2 deletions.
diff --git a/src/rp2040.spec.ts b/src/rp2040.spec.ts
@@ -50,6 +50,7 @@ import {
   opcodeSUBS2,
   opcodeSUBsp,
   opcodeSUBSreg,
+  opcodeSVC,
   opcodeSXTB,
   opcodeUXTB,
   opcodeUXTH,
@@ -70,6 +71,7 @@ const lr = 14;
 const pc = 15;
 
 const VTOR = 0xe000ed08;
+const EXC_SVCALL = 11;
 
 describe('RP2040', () => {
   it(`should initialize PC and SP according to bootrom's vector table`, () => {
@@ -1000,6 +1002,26 @@ describe('RP2040', () => {
       expect(rp2040.V).toEqual(false);
     });
 
+    it('should raise an SVCALL exception when `svc` instruction runs', () => {
+      const SVCALL_HANDLER = 0x10002000;
+      const rp2040 = new RP2040();
+      rp2040.SP = 0x20004000;
+      rp2040.PC = 0x10004000;
+      rp2040.writeUint16(0x10004000, opcodeSVC(10));
+      rp2040.registers[r0] = 0x44;
+      rp2040.writeUint32(VTOR, 0x10040000);
+      rp2040.writeUint32(0x10040000 + EXC_SVCALL * 4, SVCALL_HANDLER);
+      rp2040.writeUint16(SVCALL_HANDLER, opcodeMOVS(r0, 0x55));
+
+      rp2040.executeInstruction();
+      expect(rp2040.pendingSVCall).toEqual(true);
+
+      rp2040.executeInstruction(); // SVCall handler should run here
+      expect(rp2040.pendingSVCall).toEqual(false);
+      expect(rp2040.PC).toEqual(SVCALL_HANDLER + 2);
+      expect(rp2040.registers[r0]).toEqual(0x55);
+    });
+
     it('should execute a `sxtb r2, r2` instruction with sign bit 1', () => {
       const rp2040 = new RP2040();
       rp2040.PC = 0x10000000;

diff --git a/src/rp2040.ts b/src/rp2040.ts
@@ -111,6 +111,7 @@ export class RP2040 {
   pendingInterrupts: number = 0;
   enabledInterrupts: number = 0;
   interruptPriorities = [0xffffffff, 0x0, 0x0, 0x0];
+  pendingSVCall: boolean = false;
   interruptsUpdated = false;
 
   private executeTimer: NodeJS.Timeout | null = null;
@@ -554,6 +555,10 @@ export class RP2040 {
     // SleepOnExit(); // IMPLEMENTATION DEFINED
   }
 
+  get svCallPriority() {
+    return this.readUint32(PPB_BASE + OFFSET_SHPR2) >>> 30;
+  }
+
   exceptionPriority(n: number) {
     switch (n) {
       case EXC_RESET:
@@ -563,11 +568,11 @@ export class RP2040 {
       case EXC_HARDFAULT:
         return -1;
       case EXC_SVCALL:
-        return this.readUint32(PPB_BASE + OFFSET_SHPR2) >> 30;
+        return this.svCallPriority;
       case EXC_PENDSV:
         return (this.readUint32(PPB_BASE + OFFSET_SHPR3) >> 22) & 0x3;
       case EXC_SYSTICK:
-        return this.readUint32(PPB_BASE + OFFSET_SHPR3) >> 30;
+        return this.readUint32(PPB_BASE + OFFSET_SHPR3) >>> 30;
       default:
         if (n < 16) {
           return LOWEST_PRIORITY;
@@ -588,8 +593,14 @@ export class RP2040 {
       this.PM ? 0 : LOWEST_PRIORITY
     );
     const interruptSet = this.pendingInterrupts & this.enabledInterrupts;
+    const { svCallPriority } = this;
     for (let priority = 0; priority < currentPriority; priority++) {
       const levelInterrupts = interruptSet & this.interruptPriorities[priority];
+      if (this.pendingSVCall && priority === svCallPriority) {
+        this.pendingSVCall = false;
+        this.exceptionEntry(EXC_SVCALL);
+        return;
+      }
       if (levelInterrupts) {
         for (let interruptNumber = 0; interruptNumber < 32; interruptNumber++) {
           if (levelInterrupts & (1 << interruptNumber)) {
@@ -1290,6 +1301,11 @@ export class RP2040 {
       this.C = leftValue >= rightValue;
       this.V = (leftValue | 0) < 0 && rightValue > 0 && result > 0;
     }
+    // SVC
+    else if (opcode >> 8 === 0b11011111) {
+      this.pendingSVCall = true;
+      this.interruptsUpdated = true;
+    }
     // SXTB
     else if (opcode >> 6 === 0b1011001001) {
       const Rm = (opcode >> 3) & 0x7;

diff --git a/src/utils/assembler.spec.ts b/src/utils/assembler.spec.ts
@@ -47,6 +47,7 @@ import {
   opcodeSUBS2,
   opcodeSUBsp,
   opcodeSUBSreg,
+  opcodeSVC,
   opcodeSXTB,
   opcodeUXTB,
   opcodeUXTH,
@@ -268,6 +269,10 @@ describe('assembler', () => {
     expect(opcodeSUBS2(r3, 13)).toEqual(0x3b0d);
   });
 
+  it('should correctly encode an `svc 0` instruction', () => {
+    expect(opcodeSVC(0)).toEqual(0xdf00);
+  });
+
   it('should correctly encode an `sxtb r2, r2` instruction', () => {
     expect(opcodeSXTB(r2, r2)).toEqual(0xb252);
   });

diff --git a/src/utils/assembler.ts b/src/utils/assembler.ts
@@ -207,6 +207,10 @@ export function opcodeSUBsp(imm: number) {
   return (0b101100001 << 7) | ((imm >> 2) & 0x7f);
 }
 
+export function opcodeSVC(imm8: number) {
+  return (0b11011111 << 8) | (imm8 & 0xff);
+}
+
 export function opcodeSXTB(Rd: number, Rm: number) {
   return (0b1011001001 << 6) | ((Rm & 7) << 3) | (Rd & 7);
 }