From 3a3199fc5858b3618371b1c2a7810ecc14a3f05b Mon Sep 17 00:00:00 2001
From: Nathaniel Walizer <robots.are.love@gmail.com>
Date: Mon, 6 Jan 2025 18:04:48 -0800
Subject: [PATCH] Complete APU pulse channel; Make a few tweaks from APU tests

---
 src/apu.c | 297 +++++++++++++++++++++++++++++++++++++++---------------
 src/apu.h |  37 ++++---
 2 files changed, 239 insertions(+), 95 deletions(-)

diff --git a/src/apu.c b/src/apu.c
index 338ba8e..98e630b 100644
--- a/src/apu.c
+++ b/src/apu.c
@@ -57,18 +57,47 @@ static const void nes_apu_dmc_start(nes_apu* apu,
     nes_apu_dmc_fetch(apu, channel);
 }
 
-static void nes_apu_write_square(nes_apu_Channel* channel,
-                                 int reg, uint8_t val) {
-    APU_LOG("APU: Square %d < %02x\n", reg, val);
-    // TODO
+static inline int apu_channel_raw_timer_value(
+        nes_apu_Channel *channel) {
+    return (    channel->reg[2] |
+                (((uint16_t)channel->reg[3] & 0x7U) << 8));
 }
 
 static inline void apu_channel_update_timer(
         nes_apu_Channel *channel) {
-    channel->period = (
-            channel->reg[2] |
-            (((uint16_t)channel->reg[3] & 0x7U) << 8)
-        ) * nes_clock_cpu_div;
+    channel->period = apu_channel_raw_timer_value(channel) *
+                      nes_clock_cpu_div;
+}
+
+static void nes_apu_write_square(nes_apu_Channel* channel,
+                                 int reg, uint8_t val) {
+    APU_LOG("APU: Square %d < %02x\n", reg, val);
+
+    if (0 == reg) {
+        channel->env_period = (val & apu_Envelope_Volume);
+
+    } else if (1 == reg) {
+        channel->sweep_period = (
+                ((val & apu_Square_Period) >> 4)
+            );
+        channel->flags |= apu_Channel_Reload;
+        //channel->sweep_delay = 0;
+        //channel->step = 0;
+
+    } else if (2 <= reg) {
+        channel->reg[reg] = val;
+        channel->period = (
+                (apu_channel_raw_timer_value(channel) /*+ 1*/) * 2
+            ) * nes_clock_cpu_div;
+        if (3 == reg) {
+            //channel->flags |= apu_Channel_Reload; // TODO: No?
+//            channel->sweep_delay = 0; // TODO: No?
+//            channel->step = 7;
+//            channel->envelope = 15;
+            //channel->envelope = (channel->reg[0] & apu_Envelope_Volume);
+        }
+    }
+    // TODO?
 }
 
 static void nes_apu_write_triangle(nes_apu_Channel* channel,
@@ -78,7 +107,7 @@ static void nes_apu_write_triangle(nes_apu_Channel* channel,
     if (2 <= reg) {
         channel->reg[reg] = val;
         apu_channel_update_timer(channel);
-        if (3 == reg) channel->reload = 1;
+        if (3 == reg) channel->flags |= apu_Channel_Reload;
     }
 }
 
@@ -87,7 +116,7 @@ static void nes_apu_write_noise(nes_apu_Channel* channel,
     APU_LOG("APU: Noise %d < %02x\n", reg, val);
 
     if (reg == 0) {
-        channel->envelope = (val & apu_Envelope_Volume);
+        channel->env_period = (val & apu_Envelope_Volume);
 
     } else if (reg == 2) {
         channel->period = noise_period_lut[val & apu_Noise_Period] *
@@ -117,7 +146,7 @@ int nes_apu_init(nes_apu* apu, int clock, int frequency,
     int ret = 0;
 
     // 20 ms buffer
-    apu->blip = blip_new(frequency / 20);
+    apu->blip = blip_new(frequency / 50);
 
     if (NULL == apu->blip) {
         APU_ERR("APU: Failed to create resampler\n");
@@ -145,7 +174,9 @@ int nes_apu_init(nes_apu* apu, int clock, int frequency,
 
         apu->channels[apu_Channel_Noise].lfsr = 0x7FFFU;
 
-        apu->frame_period = clock / 240;
+        apu->frame_period = (clock / 240) +
+                            (nes_clock_cpu_div / 2);
+        apu->frame_delay = apu->frame_period;
     }
 
     return ret;
@@ -165,6 +196,7 @@ void nes_apu_reset(nes_apu* apu) {
     apu->channels[apu_Channel_Triangle].step = 31;
     apu->channels[apu_Channel_DMC].reg[1] &= 1;
     apu->channels[apu_Channel_DMC].sample = 0;
+    apu->channels[apu_Channel_DMC].mask = 0;
 }
 
 uint8_t nes_apu_read(nes_apu* apu, uint16_t addr) {
@@ -186,12 +218,94 @@ uint8_t nes_apu_read(nes_apu* apu, uint16_t addr) {
             apu->status &= ~apu_Status_Frame_Int;
         }
 
-        APU_LOG("APU: Status %02x\n", val);
+        APU_LOG("APU: Status %02x @ %d\n", val, apu->frame_time_elapsed);
     }
 
     return val;
 }
 
+static inline void nes_apu_clock_length(nes_apu_Channel* channel,
+                                        uint8_t halt_mask) {
+    if (    channel->length > 0 &&
+            !(channel->reg[0] & halt_mask)) {
+        channel->length--;
+        APU_LOG("APU: Clock Length %p -> %d\n", channel, channel->length);
+    }
+}
+
+static inline void nes_apu_clock_sweep(nes_apu_Channel* channel,
+                                       int adjust) {
+    if (channel->flags & apu_Channel_Reload) {
+        channel->flags &= ~apu_Channel_Reload;
+        channel->sweep_delay = channel->sweep_period;
+
+    } else if (channel->sweep_delay == 0) {
+        channel->sweep_delay = channel->sweep_period;
+        if (    (channel->reg[1] & apu_Square_Enable) &&
+                0 != (channel->reg[1] & apu_Square_Shift)) {
+            int delta = (
+                    apu_channel_raw_timer_value(channel) >>
+                    (channel->reg[1] & apu_Square_Shift)
+                );
+            if (channel->reg[1] & apu_Square_Negate) {
+                delta = adjust - delta;
+            }
+            channel->period += delta * nes_clock_apu_div;
+            if (channel->period < 0) {
+                channel->period = 0;
+            }
+        }
+
+    } else {
+        channel->sweep_delay--;
+    }
+}
+
+static inline void nes_apu_clock_envelope(nes_apu_Channel* channel) {
+    if (channel->flags & apu_Channel_Start) {
+        channel->flags &= ~apu_Channel_Start;
+        channel->envelope = 15;
+        channel->env_delay = channel->env_period;
+
+    } else if (channel->env_delay <= 0) {
+        channel->env_delay = channel->env_period;
+        if (    channel->envelope > 0 ||
+                (channel->reg[0] & apu_Envelope_Halt)) {
+            channel->envelope = ((channel->envelope - 1) & 0xFU);
+        }
+
+    } else {
+        channel->env_delay--;
+    }
+}
+
+static inline void nes_apu_clock_linear(nes_apu_Channel* channel) {
+    if (channel->flags & apu_Channel_Reload) {
+        channel->counter = (channel->reg[0] & apu_Triangle_Count);
+    } else if (channel->counter > 0) {
+        channel->counter--;
+    }
+    if (!(channel->reg[0] & apu_Triangle_Halt)) {
+        channel->flags &= ~apu_Channel_Reload;
+    }
+}
+
+static void nes_apu_clock_quarter_frame(nes_apu* apu) {
+    nes_apu_clock_envelope(&apu->channels[apu_Channel_Square_0]);
+    nes_apu_clock_envelope(&apu->channels[apu_Channel_Square_1]);
+    nes_apu_clock_envelope(&apu->channels[apu_Channel_Noise]);
+    nes_apu_clock_linear(&apu->channels[apu_Channel_Triangle]);
+}
+
+static void nes_apu_clock_half_frame(nes_apu* apu) {
+    nes_apu_clock_length(&apu->channels[apu_Channel_Square_0], apu_Envelope_Halt);
+    nes_apu_clock_length(&apu->channels[apu_Channel_Square_1], apu_Envelope_Halt);
+    nes_apu_clock_length(&apu->channels[apu_Channel_Triangle], apu_Triangle_Halt);
+    nes_apu_clock_length(&apu->channels[apu_Channel_Noise], apu_Envelope_Halt);
+    nes_apu_clock_sweep(&apu->channels[apu_Channel_Square_0], -1);
+    nes_apu_clock_sweep(&apu->channels[apu_Channel_Square_1], 0);
+}
+
 void nes_apu_write(nes_apu* apu, uint16_t addr, uint8_t val) {
     if (addr < nes_apu_reg_base +
                (nes_apu_chan_count * nes_apu_chan_size)) {
@@ -201,12 +315,15 @@ void nes_apu_write(nes_apu* apu, uint16_t addr, uint8_t val) {
         nes_apu_Channel* channel = &apu->channels[chan];
         if (3 == reg) {
             if (    chan != apu_Channel_DMC &&
-                    apu->status & (1 << chan)) {
+                    (apu->status & (1 << chan))) {
                 channel->length = apu_length_lut[val >> 3];
-                // TODO: Handle Envelope vs. Triangle
-                channel->envelope = 15;
-                channel->env_period = (channel->reg[0] & apu_Envelope_Volume);
-                channel->env_delay = channel->env_period;
+//                channel->step = (uint8_t)-1;
+                /*if (chan != apu_Channel_Triangle)*/ {
+                    channel->flags |= apu_Channel_Start;
+//                    channel->envelope = 15;
+                    // TODO
+//                    channel->env_delay = channel->env_period;
+                }
             }
         }
         channel->write(channel, reg, val);
@@ -215,20 +332,23 @@ void nes_apu_write(nes_apu* apu, uint16_t addr, uint8_t val) {
     } else if (addr == nes_apu_reg_status) {
         APU_LOG("APU: Enable %02x\n", val);
 
+        // Clear this now in case nes_apu_dmc_start resets it
+        apu->channels[apu_Channel_DMC].interrupt = 0;
+
         for (int chan = 0; chan < nes_apu_chan_count; ++chan) {
             if (!(val & (1 << chan))) {
                 apu->channels[chan].length = 0;
             }
         }
 
-        if (    (val & (1 << apu_Channel_DMC)) &&
-                !(apu->status & (1 << apu_Channel_DMC))) {
+        if (!(val & (1 << apu_Channel_DMC))) {
+            apu->channels[apu_Channel_DMC].mask = 0;
+
+        } else if ( (val & (1 << apu_Channel_DMC)) &&
+                    apu->channels[apu_Channel_DMC].length <= 0) {
             nes_apu_dmc_start(apu,
                               &apu->channels[apu_Channel_DMC]);
         }
-        if (!(val & (1 << apu_Channel_DMC))) {
-            apu->channels[apu_Channel_DMC].interrupt = 0;
-        }
 
         apu->status &= ~((1 << nes_apu_chan_count) - 1);
         apu->status |= (val & ((1 << nes_apu_chan_count) - 1));
@@ -240,7 +360,13 @@ void nes_apu_write(nes_apu* apu, uint16_t addr, uint8_t val) {
                          (apu_Frame_Mode | apu_Frame_Inhibit);
 
         apu->frame = 0;
-        apu->frame_delay = 0;
+        apu->frame_time_elapsed = 0;
+        apu->frame_delay = apu->frame_period;
+
+        if (val & apu_Frame_Mode) {
+            nes_apu_clock_quarter_frame(apu);
+            nes_apu_clock_half_frame(apu);
+        }
 
         if (val & apu_Frame_Inhibit) {
             apu->status &= ~apu_Status_Frame_Int;
@@ -301,7 +427,7 @@ static void nes_apu_run_dmc(nes_apu* apu,
 static inline int apu_envelope_volume(nes_apu_Channel* channel) {
     return (    (channel->reg[0] & apu_Envelope_Constant) ?
                 (channel->reg[0] & apu_Envelope_Volume) :
-                (channel->envelope) );
+                channel->envelope );
 }
 
 static void nes_apu_run_noise(nes_apu* apu,
@@ -386,35 +512,49 @@ static void nes_apu_run_triangle(nes_apu* apu,
     }
 }
 
+static const uint8_t square_sequence[4] = {
+    0b00000010,
+    0b00000110,
+    0b00011110,
+    0b11111001,
+};
 
-static inline void nes_apu_clock_length(nes_apu_Channel* channel,
-                                        uint8_t halt_mask) {
-    if (    channel->length > 0 &&
-            !(channel->reg[0] & halt_mask)) {
-        channel->length--;
+static void nes_apu_run_square(nes_apu* apu,
+                               nes_apu_Channel* channel,
+                               int cycles) {
+    if (    channel->length <= 0 ||
+            channel->period <= 0) {
+        return;
     }
-}
 
-static inline void nes_apu_clock_envelope(nes_apu_Channel* channel) {
-    if (channel->env_delay <= 0) {
-        channel->env_delay = channel->env_period;
-        if (    channel->envelope > 0 ||
-                (channel->reg[0] & apu_Envelope_Halt)) {
-            channel->envelope = ((channel->envelope - 1) & 0xFU);
+    int time = apu->time;
+
+    while (cycles > 0) {
+        int run = cycles;
+        if (run > channel->delay) {
+            run = channel->delay;
         }
-    } else {
-        channel->env_delay--;
-    }
-}
 
-static inline void nes_apu_clock_linear(nes_apu_Channel* channel) {
-    if (channel->reload) {
-        channel->counter = (channel->reg[0] & apu_Triangle_Count);
-    } else if (channel->counter > 0) {
-        channel->counter--;
-    }
-    if (!(channel->reg[0] & apu_Triangle_Halt)) {
-        channel->reload = 0;
+        channel->delay -= run;
+        time += run;
+
+        if (channel->delay <= 0) {
+            channel->delay = channel->period;
+
+            channel->step = ((channel->step + 1) & 7U);
+
+            channel->sample = ((
+                        square_sequence[channel->reg[0] >> 6] &
+                        (1 << channel->step)
+                    ) ? apu_envelope_volume(channel) : 0);
+
+            int delta = channel_update(channel);
+            if (delta) {
+                blip_add_delta(apu->blip, time, delta);
+            }
+        }
+
+        cycles -= run;
     }
 }
 
@@ -426,12 +566,15 @@ nes_apu_Result nes_apu_run(nes_apu* apu, int cycles) {
         if (run > apu->frame_delay) {
             run = apu->frame_delay;
         }
-/*
-        nes_apu_run_square(&apu->channels[apu_Channel_Square_0],
-                           apu->blip, run);
-        nes_apu_run_square(&apu->channels[apu_Channel_Square_1],
-                           apu->blip, run);
-*/
+
+        apu->frame_time_elapsed += run;
+
+        nes_apu_run_square(
+                apu, &apu->channels[apu_Channel_Square_0], run
+            );
+        nes_apu_run_square(
+                apu, &apu->channels[apu_Channel_Square_1], run
+            );
         nes_apu_run_triangle(
                 apu, &apu->channels[apu_Channel_Triangle], run
             );
@@ -442,57 +585,47 @@ nes_apu_Result nes_apu_run(nes_apu* apu, int cycles) {
         apu->frame_delay -= run;
 
         if (apu->frame_delay <= 0) {
+            APU_LOG("APU: End of quarter frame: %d\n", apu->frame_time_elapsed);
+
             int end = 0;
-            int clock = 1;
-            int half_clock = 0;
+            int quarter_frame = 1;
+            int half_frame = 0;
 
             apu->frame_delay += apu->frame_period;
 
             if (1 == apu->frame) {
-                half_clock = 1;
+                half_frame = 1;
             }
 
             if (apu->frame_reg & apu_Frame_Mode) {
                 if (3 == apu->frame) {
-                    clock = 0;
-                } else if (4 == apu->frame) {
-                    half_clock = 1;
+                    quarter_frame = 0;
+                } else if (4 <= apu->frame) {
+                    half_frame = 1;
                     end = 1;
                 }
             } else {
-                if (3 == apu->frame) {
-                    half_clock = 1;
+                if (3 <= apu->frame) {
+                    half_frame = 1;
                     end = 1;
                 }
             }
 
-            if (half_clock) {
-/*
-                nes_apu_clock_length(&apu->channels[apu_Channel_Square_0], apu_Envelope_Halt);
-                nes_apu_clock_length(&apu->channels[apu_Channel_Square_1], apu_Envelope_Halt);
-*/
-                nes_apu_clock_length(&apu->channels[apu_Channel_Triangle], apu_Triangle_Halt);
-                nes_apu_clock_length(&apu->channels[apu_Channel_Noise], apu_Envelope_Halt);
-/*
-                nes_apu_clock_sweep(&apu->channels[apu_Channel_Square_0], -1);
-                nes_apu_clock_sweep(&apu->channels[apu_Channel_Square_1], 0);
-*/
+            if (half_frame) {
+                nes_apu_clock_half_frame(apu);
             }
 
-            if (clock) {
-/*
-                nes_apu_clock_envelope(&apu->channels[apu_Channel_Square_0]);
-                nes_apu_clock_envelope(&apu->channels[apu_Channel_Square_1]);
-*/
-                nes_apu_clock_envelope(&apu->channels[apu_Channel_Noise]);
-                nes_apu_clock_linear(&apu->channels[apu_Channel_Triangle]);
+            if (quarter_frame) {
+                nes_apu_clock_quarter_frame(apu);
             }
 
             if (end) {
-                apu->frame = 0;
                 if (0 == apu->frame_reg) {
                     apu->status |= apu_Status_Frame_Int;
+                    APU_LOG("APU: Frame Interrupt @ %d\n", apu->frame_time_elapsed);
                 }
+                apu->frame = 0;
+                apu->frame_time_elapsed = 0;
             } else {
                 apu->frame++;
             }
diff --git a/src/apu.h b/src/apu.h
index be85e18..5e0f9da 100644
--- a/src/apu.h
+++ b/src/apu.h
@@ -80,6 +80,11 @@ static inline int nes_apu_channel_timer(uint8_t reg[4]) {
     return ((unsigned)reg[2] | (((unsigned)reg[3] & 7U) << 8));
 }
 
+typedef enum {
+    apu_Channel_Reload  = 0b00000001,
+    apu_Channel_Start   = 0b00000010,
+} nes_apu_Channel_Flag;
+
 typedef struct nes_apu_Channel_t {
     uint8_t reg[4];
     void(*write)(struct nes_apu_Channel_t*, int reg, uint8_t);
@@ -89,23 +94,28 @@ typedef struct nes_apu_Channel_t {
     int sample; // Current sample
     int output; // Last output (sample * gain)
 
-    uint16_t period;
+    int period;
 
     union {
-        // Square/Noise
-        struct {
-            int phase;
-            int sweep;
-            uint8_t envelope;
-            uint8_t env_period;
-            uint8_t env_delay;
-            uint16_t lfsr;
-        };
-        // Triangle
+        // Square/Triangle/Noise
         struct {
             uint8_t step;
-            uint8_t reload;
-            uint8_t counter;
+            uint8_t flags;
+            union {
+                // Square/Noise
+                struct {
+                    uint8_t envelope;
+                    uint8_t env_period;
+                    int8_t env_delay;
+                    uint8_t sweep_period;
+                    int8_t sweep_delay;
+                    uint16_t lfsr;
+                };
+                // Triangle
+                struct {
+                    uint8_t counter;
+                };
+            };
         };
         // DMC
         struct {
@@ -128,6 +138,7 @@ typedef struct {
     int frame_delay;
     int frame_period;
     int time;
+    int frame_time_elapsed;
 } nes_apu;
 
 typedef enum {