Introduce pseudo-halt

core/cpu.c

@@ -10,97 +10,86 @@
 #define CPU_HALTING   1
 #define CPU_FATAL     2
 #define CPU_FETCH     3
-#define CPU_ILLEGAL   4
+#define CPU_PHALT     4
-#define CPU_BITSTRING 5
+#define CPU_ILLEGAL   5
-#define CPU_FLOATENDO 6
+#define CPU_BITSTRING 6
-#define CPU_ADD_IMM   7
+#define CPU_FLOATENDO 7
-#define CPU_ADD_REG   8
+#define CPU_ADD_IMM   8
-#define CPU_ADDF_S    9
+#define CPU_ADD_REG   9
-#define CPU_ADDI     10
+#define CPU_ADDF_S   10
-#define CPU_AND      11
+#define CPU_ADDI     11
-#define CPU_ANDI     12
+#define CPU_AND      12
-#define CPU_BCOND    13
+#define CPU_ANDI     13
-#define CPU_CAXI     14
+#define CPU_BCOND    14
-#define CPU_CLI      15
+#define CPU_CAXI     15
-#define CPU_CMP_IMM  16
+#define CPU_CLI      16
-#define CPU_CMP_REG  17
+#define CPU_CMP_IMM  17
-#define CPU_CMPF_S   18
+#define CPU_CMP_REG  18
-#define CPU_CVT_SW   19
+#define CPU_CMPF_S   19
-#define CPU_CVT_WS   20
+#define CPU_CVT_SW   20
-#define CPU_DIV      21
+#define CPU_CVT_WS   21
-#define CPU_DIVF_S   22
+#define CPU_DIV      22
-#define CPU_DIVU     23
+#define CPU_DIVF_S   23
-#define CPU_HALT     24
+#define CPU_DIVU     24
-#define CPU_IN_B     25
+#define CPU_HALT     25
-#define CPU_IN_H     26
+#define CPU_IN_B     26
-#define CPU_IN_W     27
+#define CPU_IN_H     27
-#define CPU_JAL      28
+#define CPU_IN_W     28
-#define CPU_JMP      29
+#define CPU_JAL      29
-#define CPU_JR       30
+#define CPU_JMP      30
-#define CPU_LD_B     31
+#define CPU_JR       31
-#define CPU_LD_H     32
+#define CPU_LD_B     32
-#define CPU_LD_W     33
+#define CPU_LD_H     33
-#define CPU_LDSR     34
+#define CPU_LD_W     34
-#define CPU_MOV_IMM  35
+#define CPU_LDSR     35
-#define CPU_MOV_REG  36
+#define CPU_MOV_IMM  36
-#define CPU_MOVEA    37
+#define CPU_MOV_REG  37
-#define CPU_MOVHI    38
+#define CPU_MOVEA    38
-#define CPU_MPYHW    39
+#define CPU_MOVHI    39
-#define CPU_MUL      40
+#define CPU_MPYHW    40
-#define CPU_MULF_S   41
+#define CPU_MUL      41
-#define CPU_MULU     42
+#define CPU_MULF_S   42
-#define CPU_NOT      43
+#define CPU_MULU     43
-#define CPU_OR       44
+#define CPU_NOT      44
-#define CPU_ORI      45
+#define CPU_OR       45
-#define CPU_OUT_B    46
+#define CPU_ORI      46
-#define CPU_OUT_H    47
+#define CPU_OUT_B    47
-#define CPU_OUT_W    48
+#define CPU_OUT_H    48
-#define CPU_RETI     49
+#define CPU_OUT_W    49
-#define CPU_REV      50
+#define CPU_RETI     50
-#define CPU_SAR_IMM  51
+#define CPU_REV      51
-#define CPU_SAR_REG  52
+#define CPU_SAR_IMM  52
-#define CPU_SCH0BSD  53
+#define CPU_SAR_REG  53
-#define CPU_SCH0BSU  54
+#define CPU_SCH0BSD  54
-#define CPU_SCH1BSD  55
+#define CPU_SCH0BSU  55
-#define CPU_SCH1BSU  56
+#define CPU_SCH1BSD  56
-#define CPU_SEI      57
+#define CPU_SCH1BSU  57
-#define CPU_SETF     58
+#define CPU_SEI      58
-#define CPU_SHL_IMM  59
+#define CPU_SETF     59
-#define CPU_SHL_REG  60
+#define CPU_SHL_IMM  60
-#define CPU_SHR_IMM  61
+#define CPU_SHL_REG  61
-#define CPU_SHR_REG  62
+#define CPU_SHR_IMM  62
-#define CPU_ST_B     63
+#define CPU_SHR_REG  63
-#define CPU_ST_H     64
+#define CPU_ST_B     64
-#define CPU_ST_W     65
+#define CPU_ST_H     65
-#define CPU_STSR     66
+#define CPU_ST_W     66
-#define CPU_SUB      67
+#define CPU_STSR     67
-#define CPU_SUBF_S   68
+#define CPU_SUB      68
-#define CPU_TRAP     69
+#define CPU_SUBF_S   69
-#define CPU_TRNC_SW  70
+#define CPU_TRAP     70
-#define CPU_XB       71
+#define CPU_TRNC_SW  71
-#define CPU_XH       72
+#define CPU_XB       72
-#define CPU_XOR      73
+#define CPU_XH       73
-#define CPU_XORI     74
+#define CPU_XOR      74
-#define CPU_ANDBSU   75 /* Keep bit string ALU commands sequential */
+#define CPU_XORI     75
-#define CPU_ANDNBSU  76
+#define CPU_ANDBSU   76 /* Keep bit string ALU IDs consecutive */
-#define CPU_MOVBSU   77
+#define CPU_ANDNBSU  77
-#define CPU_NOTBSU   78
+#define CPU_MOVBSU   78
-#define CPU_ORBSU    79
+#define CPU_NOTBSU   79
-#define CPU_ORNBSU   80
+#define CPU_ORBSU    80
-#define CPU_XORBSU   81
+#define CPU_ORNBSU   81
-#define CPU_XORNBSU  82
+#define CPU_XORBSU   82
-
+#define CPU_XORNBSU  83
-/* Abstract operand types */
-#define CPU_IMP(x) -x-1
-#define CPU_DISP9  1
-#define CPU_DISP26 2
-#define CPU_IMM16S 3
-#define CPU_IMM16U 4
-#define CPU_IMM5S  5
-#define CPU_IMM5U  6
-#define CPU_MEM    7
-#define CPU_REG1   8
-#define CPU_REG2   9
 
 /* Functional operand types */
 #define CPU_LITERAL  0
@@ -206,6 +195,13 @@ static const uint8_t OPDEFS_FLOATENDO[] = {
 
 
 
+/**************************** Forward references *****************************/
+
+static int      phAssess (VB *, uint32_t, int, int32_t);
+static uint32_t phUntil  (VB *);
+
+
+
 /***************************** Callback Handlers *****************************/
 
 /* Prepare to handle an exception */
@@ -248,6 +244,10 @@ static int cpuRead(VB *sim, uint32_t address, int type, int32_t *value) {
     /* Retrieve the value from the simulation state directly */
     busRead(sim, address, type, value);
 
+    /* Check for pseudo-halt */
+    if (sim->ph.enabled && phAssess(sim, address, type, *value))
+        return 0;
+
     /* Invoke the callback if available */
     if (
         sim->onRead != NULL &&
@@ -296,6 +296,10 @@ static int cpuWrite(VB *sim, uint32_t address, int type, int32_t value) {
     int      cancel = 0;
     uint32_t cycles = 3; /* TODO: Research this */
 
+    /* Reset pseudo-halt */
+    if (sim->ph.enabled)
+        sim->ph.step = 0;
+
     /* Invoke the callback if available */
     if (
         sim->onWrite != NULL &&
@@ -482,10 +486,13 @@ static int cpuException(VB *sim) {
         /* Interrupts only */
         if ((cause & 0xFF00) == 0xFE00) {
             sim->cpu.psw.i = (cause >> 4 & 7) + 1;
+
+            /* HALT instruction */
             if (sim->cpu.halt) {
                 sim->cpu.halt = 0;
                 sim->cpu.pc  += 2;
             }
+
         }
 
         /* All exceptions */
@@ -583,6 +590,30 @@ static int cpuHalt(VB *sim) {
     return !cpuIRQ(sim);
 }
 
+/* Pseudo-halt is pending */
+static int cpuPHalt(VB *sim) {
+    int32_t value; /* Value read from memory */
+
+    /* An interrupt will be accepted */
+    if (cpuIRQ(sim)) {
+        sim->ph.step = 0;
+        return 0;
+    }
+
+    /* Monitor value has not changed */
+    busRead(sim, sim->ph.address, sim->ph.type, &value);
+    if (value == sim->ph.value) {
+        sim->cpu.clocks = phUntil(sim);
+        return 1;
+    }
+
+    /* Release pseudo-halt */
+    sim->cpu.operation = CPU_FETCH;
+    sim->cpu.step      = 0;
+    sim->ph.step       = 0;
+    return 0;
+}
+
 
 
 /**************************** Instruction Helpers ****************************/
@@ -1703,6 +1734,7 @@ static int cpuEmulate(VB *sim, uint32_t clocks) {
             case CPU_FATAL    : return 0;
             case CPU_FETCH    : brk = cpuFetch    (sim);     break;
             case CPU_HALTING  : if (cpuHalt (sim)) return 0; break;
+            case CPU_PHALT    : if (cpuPHalt(sim)) return 0; break;
 
             case CPU_ADD_IMM:       cpuADDImm (sim); break;
             case CPU_ADD_REG:       cpuADDReg (sim); break;
@@ -1826,8 +1858,14 @@ static void cpuReset(VB *sim) {
 /* Determine how many clocks are guaranteed to process */
 static uint32_t cpuUntil(VB *sim, uint32_t clocks) {
 
+    /* Pseudo-halting */
+    if (sim->cpu.operation == CPU_PHALT) {
+        if (!sim->ph.operation)
+            return clocks;
+    }
+
     /* Halting */
-    if (sim->cpu.halt) {
+    else if (sim->cpu.halt) {
         return
             sim->cpu.operation == CPU_HALTING                      &&
             !(sim->cpu.psw.id | sim->cpu.psw.ep | sim->cpu.psw.np) &&

core/pseudo-halt.c

@@ -0,0 +1,177 @@
+/* This file is included into vb.c and cannot be compiled on its own. */
+#ifdef VBAPI
+
+
+
+/********************************* Constants *********************************/
+
+/* Pseudo-halt operations */
+#define PH_NEVER  0 /* Must be zero */
+#define PH_XPSTTS 1
+
+
+
+/***************************** Module Functions ******************************/
+
+/* Activate pseudo-halt */
+static void phActivate(VB *sim, uint32_t address, int type) {
+    int range; /* Memory address range by component */
+
+    (void) type;
+
+    /* Working variables */
+    address = 0x07FFFFFF;
+    range   = address >> 24;
+
+    /* Configure CPU */
+    sim->cpu.operation = CPU_PHALT;
+    sim->ph.operation  = PH_NEVER;
+
+    /* VIP */
+    if (range == 0) {
+        address &= 0x0007FFFF;
+
+        /* I/O register */
+        switch (address) {
+            case 0x5F840: /* XPSTTS */
+                if (sim->vip.dp.disp && sim->vip.xp.xpen)
+                    sim->ph.operation = PH_XPSTTS;
+                break;
+        }
+
+    }
+
+    /* Configure CPU */
+    sim->cpu.clocks = phUntil(sim);
+}
+
+/* Test whether the current memory access matches the monitored access */
+static int phMatches(VB *sim, uint32_t address, int type, int32_t value) {
+    int match; /* Parameter match */
+    int x;     /* Iterator */
+
+    /* New memory access */
+    if (sim->ph.step == 0)
+        return 0;
+
+    /* Check memory access parameters */
+    match =
+        address     == sim->ph.address &&
+        sim->cpu.pc == sim->ph.pc      &&
+        type        == sim->ph.type    &&
+        value       == sim->ph.value
+    ;
+    if (!match || sim->ph.step != 3)
+        return match;
+
+    /* Check full CPU state */
+    for (x = 1; x < 32; x++) {
+        if (sim->ph.program[x - 1] != sim->cpu.program[x])
+            return 0;
+    }
+    return
+        sim->ph.adtre == sim->cpu.adtre                     &&
+        sim->ph.chcw  == cpuGetSystemRegister(sim, VB_CHCW) &&
+        sim->ph.eipc  == sim->cpu.eipc                      &&
+        sim->ph.eipsw == sim->cpu.eipsw                     &&
+        sim->ph.fepc  == sim->cpu.fepc                      &&
+        sim->ph.fepsw == sim->cpu.fepsw                     &&
+        sim->ph.psw   == cpuGetSystemRegister(sim, VB_PSW)
+    ;
+}
+
+
+
+/****************************** Clock Measurers ******************************/
+
+/* XPSTTS */
+static uint32_t phXPSTTS(VB *sim) {
+    uint32_t  clocks;    /* Return value */
+    uint16_t *halfwords; /* Drawing clocks this SBCOUNT */
+    uint32_t  x;         /* Iterator */
+
+    /* Drawing is underway */
+    if (sim->vip.xp.f0bsy || sim->vip.xp.f1bsy) {
+        clocks    = 0;
+        halfwords = &sim->vip.halfwords[
+            (uint32_t) sim->vip.xp.sbcount * 384 + sim->vip.xp.column];
+        for (x = sim->vip.xp.column; x < 384; x++, halfwords++)
+            clocks += *halfwords;
+        return clocks;
+    }
+
+    /* Drawing is idle */
+    switch (sim->vip.dp.step) {
+        case 0: /* 0ms - FCLK rising edge */
+            return sim->vip.dp.until;
+        case 1: /* 3ms - L*BSY rising edge */
+        case 2: /* 3ms-8ms - Display left frame buffer */
+        case 3: /* 8ms - L*BSY falling edge */
+            return sim->vip.dp.until + vipClocksMs(12);
+    }
+    /* case 4: 10ms - FCLK falling edge */
+    /* case 5: 13ms - R*BSY rising edge */
+    /* case 6: 13ms-18ms - Display right frame buffer */
+    /* case 7: 18ms - R*BSY falling edge */
+    return sim->vip.dp.until + vipClocksMs(2);
+}
+
+
+
+/***************************** Library Functions *****************************/
+
+/* Test whether to activate pseudo-halt */
+static int phAssess(VB *sim, uint32_t address, int type, int32_t value) {
+    int x; /* Iterator */
+
+    /* Memory access does not match last time */
+    if (!phMatches(sim, address, type, value))
+        sim->ph.step = 0;
+
+    /* New memory access */
+    if (sim->ph.step == 0) {
+        sim->ph.address = address;
+        sim->ph.pc      = sim->cpu.pc;
+        sim->ph.step    = 1;
+        sim->ph.type    = type;
+        sim->ph.value   = value;
+        return 0;
+    }
+
+    /* Repeated memory access, not checking full CPU state */
+    if (sim->ph.step < 2) {
+        sim->ph.step++;
+        return 0;
+    }
+
+    /* Take a snapshot of the full CPU state */
+    if (sim->ph.step == 2) {
+        for (x = 1; x < 32; x++)
+            sim->ph.program[x - 1] = sim->cpu.program[x];
+        sim->ph.adtre = sim->cpu.adtre;
+        sim->ph.chcw  = cpuGetSystemRegister(sim, VB_CHCW);
+        sim->ph.eipc  = sim->cpu.eipc;
+        sim->ph.eipsw = sim->cpu.eipsw;
+        sim->ph.fepc  = sim->cpu.fepc;
+        sim->ph.fepsw = sim->cpu.fepsw;
+        sim->ph.psw   = cpuGetSystemRegister(sim, VB_PSW);
+        sim->ph.step  = 3;
+        return 0;
+    }
+
+    /* Activate pseudo-halt */
+    phActivate(sim, address, type);
+    return 1;
+}
+
+/* Determine how long the CPU should wait before checking the monitor value */
+static uint32_t phUntil(VB *sim) {
+    switch (sim->ph.operation) {
+        case PH_XPSTTS: return phXPSTTS(sim);
+    }
+    return 0; /* PH_NEVER */
+}
+
+
+
+#endif /* VBAPI */

core/vb.c

@@ -308,6 +308,29 @@ struct VB {
         int      sample; /* Output sample index, period 417 */
     } vsu;
 
+    /* Pseudo-halt */
+    struct {
+        uint32_t address;   /* Monitor address */
+        uint8_t  enabled;   /* Pseudo-halt function is enabled */
+        uint8_t  operation; /* Monitoring operation */
+        uint8_t  step;      /* Number of consecutive matching reads */
+        int      type;      /* Memory access type */
+        int32_t  value;     /* Value read from monitor address */
+
+        /* CPU snapshot */
+        uint32_t adtre;
+        uint32_t chcw;
+        uint32_t eipc;
+        uint32_t eipsw;
+        uint32_t fepc;
+        uint32_t fepsw;
+        uint32_t pc;
+        int32_t  program[31];
+        uint32_t psw;
+        uint32_t sr29;
+        uint32_t sr31;
+    } ph;
+
     /* Other state */
     uint8_t wcr;           /* Wait controller state */
     uint8_t wram[0x10000]; /* System RAM */
@@ -348,6 +371,7 @@ static int32_t SignExtend(int32_t value, int32_t bits) {
 #include "cpu.c"
 #include "vip.c"
 #include "vsu.c"
+#include "pseudo-halt.c"
 
 
 
@@ -451,6 +475,14 @@ VBAPI uint16_t vbGetKeys(VB *sim) {
     return sim->pad.keys;
 }
 
+/* Retrieve a core option value */
+VBAPI int vbGetOption(VB *sim, int key) {
+    switch (key) {
+        case VB_PSEUDO_HALT: return sim->ph.enabled;
+    }
+    return 0;
+}
+
 /* Retrieve the most recent frame image pixels */
 VBAPI void vbGetPixels(VB *sim, void *left, int leftStrideX, int leftStrideY,
     void *right, int rightStrideX, int rightStrideY) {
@@ -553,6 +585,7 @@ VBAPI VB* vbInit(VB *sim) {
     sim->onRead          = NULL;
     sim->onSamples       = NULL;
     sim->onWrite         = NULL;
+    sim->ph.enabled      = 0;
     vbReset(sim);
     return sim;
 }
@@ -583,6 +616,9 @@ VBAPI VB* vbReset(VB *sim) {
     tmrReset(sim);
     vipReset(sim);
     vsuReset(sim);
+
+    /* Pseudo-halt */
+    sim->ph.step = 0;
     return sim;
 }
 
@@ -641,6 +677,23 @@ VBAPI uint16_t vbSetKeys(VB *sim, uint16_t keys) {
     return sim->pad.keys = keys;
 }
 
+/* Specify a new core option value */
+VBAPI int vbSetOption(VB *sim, int key, int value) {
+    switch (key) {
+        case VB_PSEUDO_HALT:
+            sim->ph.enabled = value = !!value;
+            if (!value) {
+                sim->ph.step = 0;
+                if (sim->cpu.operation == CPU_PHALT) {
+                    sim->cpu.operation = CPU_FETCH;
+                    sim->cpu.step      = 0;
+                }
+            }
+            break;
+    }
+    return value;
+}
+
 /* Specify a new value for the program counter */
 VBAPI uint32_t vbSetProgramCounter(VB *sim, uint32_t value) {
     sim->cpu.operation = CPU_FETCH;

core/vb.h

@@ -79,6 +79,9 @@ extern "C" {
 #define VB_U16 3
 #define VB_S32 4
 
+/* Option keys */
+#define VB_PSEUDO_HALT 0
+
 
 
 /*********************************** Types ***********************************/
@@ -110,6 +113,7 @@ VBAPI vbOnExecute   vbGetExecuteCallback  (VB *sim);
 VBAPI vbOnFetch     vbGetFetchCallback    (VB *sim);
 VBAPI vbOnFrame     vbGetFrameCallback    (VB *sim);
 VBAPI uint16_t      vbGetKeys             (VB *sim);
+VBAPI int           vbGetOption           (VB *sim, int key);
 VBAPI void          vbGetPixels           (VB *sim, void *left, int leftStrideX, int leftStrideY, void *right, int rightStrideX, int rightStrideY);
 VBAPI uint32_t      vbGetProgramCounter   (VB *sim);
 VBAPI int32_t       vbGetProgramRegister  (VB *sim, int index);
@@ -129,6 +133,7 @@ VBAPI vbOnExecute   vbSetExecuteCallback  (VB *sim, vbOnExecute callback);
 VBAPI vbOnFetch     vbSetFetchCallback    (VB *sim, vbOnFetch callback);
 VBAPI vbOnFrame     vbSetFrameCallback    (VB *sim, vbOnFrame callback);
 VBAPI uint16_t      vbSetKeys             (VB *sim, uint16_t keys);
+VBAPI int           vbSetOption           (VB *sim, int key, int value);
 VBAPI uint32_t      vbSetProgramCounter   (VB *sim, uint32_t value);
 VBAPI int32_t       vbSetProgramRegister  (VB *sim, int index, int32_t value);
 VBAPI vbOnRead      vbSetReadCallback     (VB *sim, vbOnRead callback);

core/vip.c

@@ -42,14 +42,14 @@ static const uint8_t BG_TEMPLATES[][64] = {
 
 /* 8-bit color magnitude by brightness level */
 static const uint8_t BRIGHT8[] = {
-      0, 10, 16, 21, 25, 30, 33, 37, 40, 44, 47, 50, 53, 56, 59, 61,
+      0,  2,  4,  6,  8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30,
-     64, 67, 69, 72, 74, 77, 79, 82, 84, 86, 89, 91, 93, 95, 97,100,
+     32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62,
-    102,104,106,108,110,112,114,116,118,120,122,124,126,128,130,131,
+     64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88, 90, 92, 94,
-    133,135,137,139,141,142,144,146,148,149,151,153,155,156,158,160,
+     96, 98,100,102,104,106,108,110,112,114,116,118,120,122,124,126,
-    161,163,165,166,168,170,171,173,175,176,178,179,181,183,184,186,
+    129,131,133,135,137,139,141,143,145,147,149,151,153,155,157,159,
-    187,189,190,192,194,195,197,198,200,201,203,204,206,207,209,210,
+    161,163,165,167,169,171,173,175,177,179,181,183,185,187,189,191,
-    212,213,215,216,217,219,220,222,223,225,226,227,229,230,232,233,
+    193,195,197,199,201,203,205,207,209,211,213,215,217,219,221,223,
-    235,236,237,239,240,241,243,244,246,247,248,250,251,252,254,255
+    225,227,229,231,233,235,237,239,241,243,245,247,249,251,253,255
 };
 
 
@@ -68,9 +68,8 @@ static int32_t vipReadPalette(uint8_t *entries) {
 
 /* Raise an interrupt request */
 static void vipThrow(VB *sim, uint16_t cause) {
-    if (!(sim->vip.intenb & cause))
-        return;
     sim->vip.intpnd |= cause;
+    if (sim->vip.intenb & cause)
         sim->cpu.irq |= 0x0010;
 }
 
@@ -422,10 +421,9 @@ static void vipTransferColumn(VB *sim, int32_t eye) {
 
     /* Output is disabled */
     if ((sim->vip.dp.disp & sim->vip.dp.synce) == 0) {
-        for (z = 0; z < 0x15000; z += 4) {
+        for (y = 0; y < 224; y += 16)
-            busWriteBuffer(dest, VB_S32, 0);
+        for (z = 0; z < 16; z++, dest += 384)
-            dest += 4;
+            *dest = 0;
-        }
         return;
     }
 
@@ -838,7 +836,10 @@ static void vipDrawWorld(VB *sim, uint8_t *world, uint16_t attr) {
                         bx &= bgw;
                         by &= bgh;
                     }
-                    else cell = over;
+                    else {
+                        cell = over;
+                        continue; /* TODO: Research overplane */
+                    }
                 }
 
                 /* Locate the cell in the BG map */

core/vsu.c

@@ -164,7 +164,7 @@ static void vsuEmulateChannel(VB *sim, int index, uint32_t clocks) {
             if (!chan->int_.enb)
                 return;
             sim->vsu.freqmod.clocks = (uint32_t) sim->vsu.freqmod.interval *
-                (sim->vsu.freqmod.clk == 0 ? 19201 : 153610);
+                (sim->vsu.freqmod.clk == 0 ? 19200 : 153600);
         }
 
     } while (clocks != 0);
@@ -207,6 +207,8 @@ 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 (index == 4)
+        vsuNextFreqMod(sim, chan);
 
     /* Configure state */
     chan->env.clocks = 307220 * ((uint32_t) chan->env.interval + 1);
@@ -243,6 +245,8 @@ static void vsuWriteFQH(VB *sim, int index, uint16_t value) {
     value = value << 8 & 0x0700;
     chan->freq.current = (chan->freq.current & 0x00FF) | value;
     chan->freq.written = (chan->freq.written & 0x00FF) | value;
+    if (index == 4)
+        vsuNextFreqMod(sim, chan);
 }
 
 /* Write a value to S*FQL */
@@ -250,6 +254,8 @@ static void vsuWriteFQL(VB *sim, int index, uint8_t value) {
     Channel *chan = &sim->vsu.channels[index];
     chan->freq.current = (chan->freq.current & 0x0700) | value;
     chan->freq.written = (chan->freq.written & 0x0700) | value;
+    if (index == 4)
+        vsuNextFreqMod(sim, chan);
 }
 
 /* Write a value to S*INT */
@@ -274,7 +280,7 @@ static void vsuWriteINT(VB *sim, int index, uint8_t value) {
     }
     if (index == 4) {
         sim->vsu.freqmod.clocks = (uint32_t) sim->vsu.freqmod.interval *
-            (sim->vsu.freqmod.clk ? 153610 : 19201);
+            (sim->vsu.freqmod.clk == 0 ? 19200 : 153600);
         sim->vsu.freqmod.sample = 0;
     }
 
@@ -299,12 +305,12 @@ static void vsuWriteSWP(VB *sim, uint8_t value) {
     /* Parse fields */
     sim->vsu.freqmod.clk      = value >> 7 & 1;
     sim->vsu.freqmod.dir      = value >> 3 & 1;
-    sim->vsu.freqmod.interval = value >> 4 & 15;
+    sim->vsu.freqmod.interval = value >> 4 & 7;
     sim->vsu.freqmod.shift    = value      & 7;
 
     /* Configure state */
     clocks = (uint32_t) sim->vsu.freqmod.interval *
-        (sim->vsu.freqmod.clk ? 153610 : 19201);
+        (sim->vsu.freqmod.clk == 0 ? 19200 : 153600);
     if (clocks < sim->vsu.freqmod.clocks)
         sim->vsu.freqmod.clocks = clocks;
 }