Fix handler address for FIV exception

Reviewed-on: #3

core/cpu.c

@@ -495,7 +495,7 @@ static int cpuException(VB *sim) {
         sim->cpu.eipc     = sim->cpu.pc;
         sim->cpu.psw.ep   = 1;
         sim->cpu.nextPC   = 0xFFFF0000 |
-            ((cause & 0xFFF0) == 0xFF60 ? 0xFF60 : cause & 0xFFF0);
+            ((cause & 0xFFE0) == 0xFF60 ? 0xFF60 : cause & 0xFFF0);
     }
 
     /* All exceptions */

core/vsu.c

@@ -132,14 +132,19 @@ static void vsuEmulateChannel(VB *sim, Channel *chan) {
     }
 
     /* Envelope modification */
-    if (chan->env.enb && !chan->env.modmask && chan->env.clocks == 0) {
+    if (chan->env.clocks == 0) {
+        uint8_t new_envelope = chan->env.value;
         if (chan->env.dir == 0 && chan->env.value != 0)
-            chan->env.value--;
+            new_envelope--;
         else if (chan->env.dir == 1 && chan->env.value != 15)
-            chan->env.value++;
+            new_envelope++;
-        else if (chan->env.rep)
+        else if (chan->env.rep && chan->env.modmask != 2) {
-            chan->env.value = chan->env.reload;
+            new_envelope = chan->env.reload;
-        else chan->env.modmask = 1;
+            chan->env.modmask = 0;
+        } else if (!chan->env.modmask)
+            chan->env.modmask = 1;
+        if (chan->env.enb && !chan->env.modmask)
+            chan->env.value = new_envelope;
         chan->env.clocks = ((uint32_t) chan->env.interval + 1) * 307220;
     }
 
@@ -215,6 +220,7 @@ static void vsuWriteEV0(VB *sim, int index, uint8_t value) {
     chan->env.interval = value      &  7;
     chan->env.value    = value >> 4 & 15;
     chan->env.reload   = chan->env.value;
+    if (chan->env.modmask == 1) chan->env.modmask = 2;
     if (index == 4) {
         chan->freqmod = vsuCheckFreqMod(sim);
         vsuNextFreqMod(sim, chan);
@@ -231,6 +237,12 @@ static void vsuWriteEV1(VB *sim, int index, uint8_t value) {
     /* Parse fields */
     chan->env.enb = value      & 1;
     chan->env.rep = value >> 1 & 1;
+    if (
+        !chan->env.rep && (
+            (chan->env.reload ==  0 && chan->env.dir == 0) ||
+            (chan->env.reload == 15 && chan->env.dir == 1)
+        )
+    ) chan->env.modmask = 1;
 
     /* Processing by channel */
     switch (index) {
@@ -286,7 +298,12 @@ static void vsuWriteINT(VB *sim, int index, uint8_t value) {
     /* Update state */
     chan->int_.clocks = 76805 * ((uint32_t) chan->int_.interval + 1);
     chan->env.modmask = 0;
-    if (chan->env.enb)
+    if (
+        !chan->env.rep && (
+            (chan->env.reload ==  0 && chan->env.dir == 0) ||
+            (chan->env.reload == 15 && chan->env.dir == 1)
+        )
+    ) chan->env.modmask = 1;
     chan->env.clocks = 307220 * ((uint32_t) chan->env.interval + 1);
     if (index != 5) {
         chan->wave.sample = 0;
@@ -377,7 +394,7 @@ static void vsuEmulate(VB *sim, uint32_t clocks) {
 
                     /* Clocks until next state change */
                     chan->until = chan->clocks;
-                    if (chan->env.enb && !chan->env.modmask &&
+                    if (chan->env.enb && chan->env.modmask != 2 &&
                         chan->env.clocks < chan->until)
                         chan->until = chan->env.clocks;
                     if (chan->int_.auto_ && chan->int_.clocks < chan->until)
@@ -387,7 +404,7 @@ static void vsuEmulate(VB *sim, uint32_t clocks) {
 
                     /* Manage clocks */
                     chan->clocks -= chan->until;
-                    if (chan->env.enb && !chan->env.modmask)
+                    if (chan->env.enb && chan->env.modmask != 2)
                         chan->env.clocks -= chan->until;
                     if (chan->int_.auto_)
                         chan->int_.clocks -= chan->until;