2 files changed, 71 insertions, 0 deletions
diff --git a/src/cpu/instruction.spec.ts b/src/cpu/instruction.spec.ts
index c3508e6..0d0d566 100644
--- a/src/cpu/instruction.spec.ts
+++ b/src/cpu/instruction.spec.ts
@@ -179,6 +179,56 @@ describe('avrInstruction', () => {
     expect(cpu.cycles).toEqual(3);
   });
 
+  it('should execute `ELPM` instruction', () => {
+    cpu = new CPU(new Uint16Array(0x20000));
+    loadProgram('ELPM');
+    cpu.data[30] = 0x50; // Z <- 0x50
+    cpu.data[0x3b] = 0x2; // RAMPZ <- 2
+    cpu.progBytes[0x20050] = 0x62; // value to be loaded
+    avrInstruction(cpu);
+    expect(cpu.data[0]).toEqual(0x62); // check that value was loaded to r0
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+  });
+
+  it('should execute `ELPM r5, Z` instruction', () => {
+    cpu = new CPU(new Uint16Array(0x20000));
+    loadProgram('ELPM r5, Z');
+    cpu.data[30] = 0x11; // Z <- 0x11
+    cpu.data[0x3b] = 0x1; // RAMPZ <- 1
+    cpu.progBytes[0x10011] = 0x99; // value to be loaded
+    avrInstruction(cpu);
+    expect(cpu.data[5]).toEqual(0x99); // check that value was loaded to r5
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+  });
+
+  it('should execute `ELPM r6, Z+` instruction', () => {
+    cpu = new CPU(new Uint16Array(0x20000));
+    loadProgram('ELPM r6, Z+');
+    cpu.data[30] = 0xff; // Z <- 0xffff
+    cpu.data[31] = 0xff; // ...
+    cpu.data[0x3b] = 0x2; // RAMPZ <- 2
+    cpu.progBytes[0x2ffff] = 0x22; // value to be loaded
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[6]).toEqual(0x22); // check that value was loaded to r6
+    expect(cpu.data[30]).toEqual(0x0); // verify that Z was incremented
+    expect(cpu.data[31]).toEqual(0x0); // verify that Z was incremented
+    expect(cpu.data[0x3b]).toEqual(3); // verify that RAMPZ was incremented
+  });
+
+  it('should clamp RAMPZ when executing `ELPM r6, Z+` instruction', () => {
+    cpu = new CPU(new Uint16Array(0x20000));
+    loadProgram('ELPM r6, Z+');
+    cpu.data[30] = 0xff; // Z <- 0xffff
+    cpu.data[31] = 0xff; // ...
+    cpu.data[0x3b] = 0x3; // RAMPZ <- 3
+    avrInstruction(cpu);
+    expect(cpu.data[0x3b]).toEqual(0x0); // verify that RAMPZ was reset to zero
+  });
+
   it('should execute `ICALL` instruction', () => {
     loadProgram('ICALL');
     cpu.data[94] = 0;
diff --git a/src/cpu/instruction.ts b/src/cpu/instruction.ts
index 7e14aad..e786bf4 100644
--- a/src/cpu/instruction.ts
+++ b/src/cpu/instruction.ts
@@ -209,6 +209,27 @@ export function avrInstruction(cpu: ICPU) {
     sreg |= 128 === value ? 8 : 0;
     sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
     cpu.data[95] = sreg;
+  } else if (opcode === 0x95d8) {
+    /* ELPM, 1001 0101 1101 1000 */
+    const rampz = cpu.data[0x3b];
+    cpu.data[0] = cpu.progBytes[(rampz << 16) | cpu.dataView.getUint16(30, true)];
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x9006) {
+    /* ELPM(REG), 1001 000d dddd 0110 */
+    const rampz = cpu.data[0x3b];
+    cpu.data[(opcode & 0x1f0) >> 4] =
+      cpu.progBytes[(rampz << 16) | cpu.dataView.getUint16(30, true)];
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x9007) {
+    /* ELPM(INC), 1001 000d dddd 0111 */
+    const rampz = cpu.data[0x3b];
+    const i = cpu.dataView.getUint16(30, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.progBytes[(rampz << 16) | i];
+    cpu.dataView.setUint16(30, i + 1, true);
+    if (i === 0xffff) {
+      cpu.data[0x3b] = (rampz + 1) % (cpu.progBytes.length >> 16);
+    }
+    cpu.cycles += 2;
   } else if ((opcode & 0xfc00) === 0x2400) {
     /* EOR, 0010 01rd dddd rrrr */
     const R = cpu.data[(opcode & 0x1f0) >> 4] ^ cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];